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Abstract 

The  objective  of  this  work  is  to  develop  an  alternative  Inertial  Navigation  Sys¬ 
tem  (INS)  aiding  source  other  than  the  Global  Positioning  System  (GPS)  while  pre¬ 
serving  the  autonomy  of  the  integrated  navigation  system.  It  is  proposed  to  develop 
a  modernized  method  of  aerial  navigation  using  driftmeter  measurements.  Use  of  an 
Electro-Optical  (E/O)  system  (LANTIRN  or  Sniper)  for  ground  feature  tracking  by 
the  pilot,  and  an  independent  altitude  sensor  (baro  altimeter  or  radar  altimeter)  in 
conjunction  with  the  INS,  is  envisioned.  The  idea  is  to  use  measurements  taken  by 
the  pilot  using  the  E/O  system  to  aid  an  existing  INS,  thus  automating  the  naviga¬ 
tor’s  function.  The  pilot  will  track  a  ground  feature  with  the  E/O  system,  while  the 
aircraft  is  on  autopilot  holding  constant  airspeed,  altitude,  and  heading  during  an  INS 
aiding  session.  The  ground  feature  measurements  from  the  E/O  system  and  the  INS 
output  will  be  used  to  form  measurements  provided  to  a  linear  Kalman  Filter  (KF) 
running  on  the  navigation  computer  to  accomplish  the  INS  aiding  action.  Aiding  the 
INS  will  be  periodically  repeated  as  operationally  permissible  under  pilot  discretion. 
The  structured  environment:  constant  speed,  constant  altitude,  and  wings  level  flight 
while  on  autopilot  justifies  the  linearization  of  the  KF  kinematic  and  measurement 
equations  used  during  an  INS  aiding  session.  Due  to  justified  linearization  conditions, 
little  to  no  modeling  error  will  be  present  when  implementing  a  linear  Kalman  filter. 
Therefore,  the  strength  of  the  INS  aiding  action  will  be  exclusively  determined  by 
the  prevailing  degree  of  observability.  It  is  shown  that  after  one  short  aiding  period 
a  60%  reduction  in  position  covariance  is  realized. 


IV 


AFIT-ENG-MS-15-M-015 


My  thesis  is  dedicated  to  all  who  have  positively  impacted  my  life,  preparing  me  for 
the  rigor  associated  with  a  technical  masters.  Family,  friends,  and  mentors  who 
have  made  an  impression  on  me,  thank  you. 


v 


Acknowledgements 


Special  recognition  to  my  advisor,  Dr.  Meir  Pachter  for  all  his  extra  hours  spent 
explaining  concepts  and  helping  me  to  knock  down  roadblocks.  My  committee  mem¬ 
bers,  Dr.  Kyle  Kauffman  and  Dr.  John  Raquet  provided  me  with  research  guidance 
and  perspective,  along  with  valuable  skills  necessary  to  accomplish  this  research  from 
my  time  in  their  classroom.  The  students  in  the  GNC  track  at  AFIT,  our  long  tech¬ 
nical  discussions  over  coffee  or  a  rip  it  will  be  some  of  my  fondest  memories  from  this 
research  experience. 


Table  of  Contents 


Page 

Abstract . iv 

Acknowledgements . vi 

List  of  Figures . ix 

List  of  Tables . xi 

I.  Introduction . 1 

1.1  Motivation . 1 

1.2  Thesis  Organization  . 4 

II.  Mathematical  Background . 5 

2.1  Notation . 5 

2.2  Coordinate  Reference  Frames . 6 

True  Inertial  Frame . 6 

Earth-fixed  Inertial  Frame . 7 

Earth  Centered  Earth  Fixed  Frame . 7 

Earth-Fixed  Navigation  Frame . 8 

Body  Frame . 9 

Camera  Frame . 10 

2.3  Coordinate  Transformations  . 11 

Euler  Angle  Rotations  . 11 

Direction  Cosine  Matrix . 13 

Quaternions . 14 

2.4  Inertial  Navigation  . 15 

Accelerometers . 16 

Gyroscopes . 17 

Strapdown  Inertial  Navigation  . 18 

INS  Error  Sources . 19 

INS  Error  Equations . 21 

2.5  Driftmeter . 22 

2.6  Modeling . 25 

Deterministic  Modeling  . 26 

Stochastic  Modeling . 27 

Measurement  Models  . 30 

2.7  Kalman  Filtering . 34 

Direct  State  Kalman  Filtering . 35 

Indirect  State  Kalman  Filtering . 35 

Kalman  Filter  Equations . 37 

vii 


Page 


2.8  Digital  Imaging . 41 

Camera  Models . 41 

Camera  Noise  Model . 43 

Image  Registration . 43 

2.9  Related  Research . 44 

Inertial  Navigation  System  Aiding  Using  Vision  . 44 

Fusion  of  Low-Cost  Imaging  and  Inertial  Sensors  for 

Navigation  . 45 

A  Covariance  Analysis  of  Vision- Aided  Inertial 

Navigation:  3-D  Case . 45 

INS  Aiding  Using  Passive  Bearings-Only  Measurements 

Of  An  Unknown  Stationary  Ground  Object . 45 

III.  Methodology  . 47 

3.1  Experiment . 48 

3.2  Navigation  Scenario  . 48 

Non-Dimensionalization . 49 

Flight  Conditions . 50 

Measurement  Geometry . 50 

INS  Error  Model  -  Dynamics . 51 

3.3  Method  1:  Driftmeter  Replication . 59 

INS  Error  Model  Robustness . 59 

Measurement  Model  -  Measurement  Equation . 60 

Kalman  Filter  . 62 

3.4  Method  2:  Contemporary  Driftmeter . 64 

Measurement  Model  -  Measurement  Equation . 64 

Linearization  of  the  Measurement  Equation . 69 

Altitude  Aiding:  Barometric  Altimeter  Measurement . 70 

Kalman  Filter  . 72 

Navigation  State  Kalman  Filter . 74 

IV.  Results  . 76 

4.1  Navigator  Replication . 76 

4.2  INS  Error  Model  Robustness  to  Axis  Acceleration  . 79 

4.3  Contemporary  Driftmeter  . 80 

4.4  Navigation  State  Kalman  Filter . 85 

V.  Conclusion . 86 

5.1  Conclusions  -  Contributions . 86 

5.2  Recommendations  for  Future  Work . 87 

Bibliography  . 88 

viii 


List  of  Figures 


Figure  Page 

1  Earth  Centered  Earth  Fixed  Frame  . 8 

2  East  North  Up  Navigation  Reference  Frame . 9 

3  Aircraft  Body  Frame . 10 

4  Camera  Frame . 11 

5  Direction  Cosine  Matrix  (DCM)  Element  Cosine 

Between  Axes . 14 

6  Simple  Accelerometer  Diagram . 17 

7  Two- Axis  Gyroscope  Schematic  Diagram . 18 

8  Navigator  Locating  Ground  Feaures  with  Driftmeter . 23 

9  Typical  Driftmeter . 24 

10  Wind  Drift  Concept . 25 

11  General  Angle  Measurement  Model  . 31 

12  Direct  State  Inertial  Navigation  Kalman  Filter  Block 

Diagram . 35 

13  Indirect  State  Feedforward  Inertial  Navigation  Kalman 

Filter  Block  Diagram . 36 

14  Indirect  State  Feedback  Inertial  Navigation  Kalman 

Filter  Block  Diagram . 37 

15  Kalman  Filter  Time  Defined  . 38 

16  Indirect  State  Feedback  Inertial  Navigation  Kalman 

Filter  Block  Diagram . 38 

17  Image  System  Model . 42 

18  Pinhole  Camera  Model . 42 

19  Navigation  Geometry  of  Measurement  Arrangement . 51 


IX 


Figure  Page 

20  Pilot  Geolocating  and  Then  Tracking  a  Ground  Feature 

(P)  Through  a  Measurement  Epoch . 65 

21  Calculated  Svx  Measurement  Concept  . 75 

22  X  Pos,  Vel,  Accel  Bias  Error  -  Full  Channel  Navigator 

Replication  (KF  initialized  with  NO  cross-correlation) . 77 

23  X  Pos,  Vel,  Accel  Bias  Error  -  Full  Channel  Navigator 

Replication  (KF  initialized  with  cross-correlation) . 78 

24  X  Velocity  Error  -  Ground  Feature  Location  Known . 81 

25  Ground  Feature  Geolocation  Track  -  Ground  Feature 

Location  Known . 82 

26  X  Velocity  Error  -  Ground  Feature  Location  Unknown . 83 

27  X  Velocity  Error  -  Ground  Feature  Location  Unknown  - 

Zoomed  In . 83 

28  X  Accelerometer  Bias  Error  -  Ground  Feature  Location 

Unknown  -  Zoomed  In . 84 

29  Ground  Feature  Geolocation  Track  -  Ground  Feature 

Location  Unknown . 84 

30  Reduced  State  KF  with  calculated  5vx  -  X  Velocity  Error . 85 

31  Reduced  State  KF  with  calculated  Svx  -  X  Position  Error . 85 


x 


List  of  Tables 


Table  Page 

1  Navigator  Driftmeter  Replication  Non-Dimensionaliztion . 49 

2  Steady-State  Flight  Constants . 49 

3  Steady-State  Flight  Constants . 60 

4  INS  Position  Error  Difference  Ax  Results . 80 


PILOT-ASSISTED  INERTIAL  NAVIGATION  SYSTEM  AIDING 
USING  BEARINGS-ONLY  MEASUREMENTS  TAKEN  OVER  TIME 

I.  Introduction 


1.1  Motivation 

Since  the  wide-spread  use  of  the  GPS  in  military  applications,  alternate  sources 
of  precision  navigation  were  effectively  neglected  due  to  the  accuracy  and  technology 
development  associated  with  GPS  integration.  The  dependence  on  GPS  was  driven 
by  the  precision  and  reliability  observed  in  multi-faceted  military  applications.  As 
the  Erst  nation  to  design,  manufacture,  and  proliferate  a  Global  Navigation  Satellite 
System  (GNSS),  we  have  been  afforded  unparalleled  navigation  and  timing  capabil¬ 
ities  never  before  seen  in  the  world.  Satisfying  precision  navigation  requirements  for 
ground,  flight,  and  sea  based  missions  spawned  the  integration  and  improvement  of 
GPS  technology  into  virtually  all  applicable  military  technologies. 

The  ease  associated  with  GPS  receiver  integration  for  precise  timing  and  navi¬ 
gation  has  become  a  hidden  standard  for  technology  development  in  military  and 
civilian  applications.  Pushing  all  in  on  GPS  technology  for  navigation,  coupled  with 
technology  development  saturation  in  support  of  the  War  on  Terror,  the  United  States 
military  recognizes  the  need  to  establish  an  alternate  precision  navigation  source.  The 
challenge  is  evident  when  comparing  navigation  accuracy  solutions  to  GPS,  along  with 
a  source  as  environmentally  robust  as  GPS. 

With  the  high  volume  product  use  of  GPS  technology  in  military  applications, 
senior  leaders  identified  the  need  to  explore  alternative  precision  navigation  technolo- 
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gies  other  than  GPS.  Former  Secretary  of  the  Air  Force  General  Norton  A.  Schwartz 
advocated  for  the  reduction  of  dependence  in  GPS  in  2010  at  the  Fletcher  Conference 
on  National  Security  Strategy  and  Policy:  “It  seems  critical,  to  me,  that  the  Joint 
Force  should  reduce  its  dependence  on  GPS-aided  precision  navigation  and  timing,  al¬ 
lowing  it  to  ultimately  become  less  vulnerable,  yet  equally  precise,  and  more  resilient” 
[20].  The  active  sensor  nature  of  GPS  lends  itself  to  challenges  including  spoofing, 
jamming,  and  coverage  outages.  Spoofing  or  jamming  of  the  GPS  signal  is  a  feasible 
concern  adversaries  have  the  ability  to  exploit.  However,  the  fidelity  and  robustness 
of  military  GPS  does  provide  degrees  of  protection  against  spoofing  and  jamming 
in  specific  conditions.  Aside  from  hostile  navigation  interference,  receive  availability 
of  the  GPS  signal  is  either  constrained  or  not  possible  when  under  cover  (bridges, 
caves,  trees,  etc.)  and  in  crowded  Radio  Frequency  (RF)  reflective  environments  like 
cities  or  urban  environments  where  multi-path  errors  are  increased.  Therefore,  much 
inspiration  has  been  drawn  from  biological  inspired  navigation.  Inspired  from  human 
vision,  the  passive  nature  of  an  Electro-Optical  (E/O)  system  is  advantageous  to 
explore  as  an  alternative  inertial  aiding  source.  Going  back  to  the  classical  naviga¬ 
tion  equipment,  the  self  contained  INS,  coupled  with  biological  inspired  navigation 
has  become  a  growing  research  held  striving  for  an  alternative  precision  navigation 
source  independent  of  GPS  [6]. 

There  has  been  a  strong  navigation  community  movement  towards  vision-aided 
navigation  with  the  improvement  in  sensing  capabilities,  and  decreased  size,  weight, 
power,  and  cost  constrains  associated  with  inertial  systems.  The  vision  aided  nav¬ 
igation  problem  begins  with  the  use  of  a  passive  E/O  (Electro-Optical)  sensor  to 
track  features  in  the  scene  to  aid  an  inertial  system.  The  elements  of  these  research 
efforts  strongly  hinge  on  the  ability  for  computer  vision  algorithms  to  identify  and 
track  distinct  features  in  the  scene.  However,  there  are  still  a  multitude  of  research 
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opportunities  on  the  filtering  and  modeling  aspects  of  the  problem,  specifically  with 
the  INS.  Success  has  been  made  with  feature  based  tracking  for  inertial  system  aid¬ 
ing  using  stochastic  projection  in  a  tightly  coupled  implementation  for  navigation 
[24]  where  Monte  Carlo  simulations  were  run  prior  to  experimental  testing  with  a 
binocular  vision  system  aiding  a  tactical  and  consumer  grade  Inertial  Measurement 
Unit  (IMU). 

During  World  War  II  aircraft  like  the  B-17  and  B-24  were  equipped  with  a  sighting 
device  operated  by  the  navigators  known  as  the  driftmeter.  Sitting  in  the  glazed  nose 
of  the  aircraft  using  the  dirftmeter,  the  navigator  was  able  to  calculate  the  ground 
speed  and  the  wind  vector.  In  a  time  where  navigation  using  dead  reckoning  or 
pilotage  were  the  main  options,  the  driftmeter  was  a  revolutionary  navigation  device. 
Further  technical  explanation  of  the  driftmeter  will  be  presented  in  Chapter  II. 

Inertial  systems  installed  and  operating  on  aircraft  are  susceptible  to  an  accumu¬ 
lation  of  errors  over  time  and  require  an  update  source  for  precision  navigation.  In 
the  event,  GPS  is  not  available  to  provide  inertial  system  updates,  a  common  prac¬ 
tice  is  Initial  Point  (IP)  updates.  Surveyed  landmarks  are  logged  in  memory  prior  to 
flight  and  the  pilot  must  overfly  these  points,  and  conduct  an  absolute  inertial  system 
update.  The  nature  of  IP  updates  can  lead  to  unquantified  bias  errors  varying  be¬ 
tween  pilots  as  the  IP  update  is  conducted  based  on  a  visual  flyover.  Also,  pilots  are 
constrained  to  the  surveyed  landmarks  pre-loaded  for  absolute  IP  updates.  Allowing 
the  pilot  navigation  freedom  in  a  setting  where  GPS  is  not  available  and  IP  updates 
are  not  possible  is  the  goal.  Herein  lies  the  motivation  for  this  research  effort;  can 
a  pilot  track  an  arbitrary  ground  feature  to  gain  a  relative  inertial  system  update 
improving  the  navigation  solution. 

Investigating  the  strength  of  the  aiding  action  driftmeter  operators  were  able  to 
provide  serves  as  the  starting  point  for  this  research.  In  the  process  of  replicating 
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driftmeter  operations,  an  interesting  subject  of  observability  through  correlation  was 
uncovered.  Where  observability  is  a  deterministic  control  system  concept  and  corre¬ 
lation  is  a  stochastic  modeling  concept.  Knowing  that  the  system  dynamics  is  the 
link  between  observability  and  correlation  for  a  linear  system,  this  research  pushes  to 
understand  the  benefits  available  from  the  gained  observability. 

1.2  Thesis  Organization 

Here  in  Chapter  I,  the  introduction,  a  basic  overview  of  the  motivation  is  presented 
as  contextual  grounds  for  the  reminder  of  the  document.  Chapter  II  covers  the 
background  concepts  necessary  to  understand  the  research  along  with  the  literature 
review.  In  Chapter  III,  the  methodology  used  to  construct  simulation  and  analysis 
is  outlined.  Results  and  analysis  of  simulations  are  covered  in  Chapter  IV.  Lastly, 
Chapter  V  draws  conclusions  and  provides  insight  on  future  research  extensions  and 
applications. 
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II.  Mathematical  Background 


In  this  chapter,  a  detailed  overview  of  the  mathematics  and  physics  relating  to 
vision  aided  inertial  navigation  will  be  presented.  Understanding  or  familiarity  with 
the  concepts  presented  will  enable  comprehension  of  the  inertial  navigation  research 
covered  here  within. 

2.1  Notation 

For  convention  purposes,  the  following  are  a  list  and  explanation  of  mathematical 
notation  used  in  this  thesis. 

Scalars:  upper  or  lower  case  italicized  letters,  (e.g.,  k  or  K ). 

Vectors:  lower  case  bold  letters  or  symbols,  (e.g.,  a).  All  vectors  referenced  will  be 
column  vectors  with  ap)  being  the  element  in  the  ith  row  of  the  column  vector  a. 
Matrices:  upper  case  letters  or  symbols  in  bold  font  (e.g.,  X).  X t(itj )  is  the  scalar 
element  of  the  ith  row  and  the  jth  column  of  the  matrix  X  and  t  is  the  associated 
time. 

Transpose:  transpose  of  a  vector  or  matrix  will  be  defined  with  the  superscript  T 
(e.g.,  aT  or  XT). 

Estimated  Variables:  estimates  of  random  variables,  vectors,  or  matrices,  will  be 
identified  with  the  hat  character  (e.g.,  x,  a,  X). 

Calculated  Variables:  variables,  vectors,  or  matrices,  computed  which  contain 
errors  will  be  identified  with  the  tilde  character  (e.g.,  x,a,  X) 

Measured  Variables:  variables  measured  which  contain  errors  will  be  identified 
with  the  bar  character  (e.g.,x,  a,  X) 

Direction  Cosine  Matrix  (DCM):  a  transformation  from  frame  a  to  frame  b  is 
denoted  by  Cba 
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Reference  Frame:  the  reference  frame  a  vector  is  referenced  in  will  be  denoted  by 
a  superscript  letter  in  parentheses,  describing  the  reference  frame  (e.g.,  where 
vector  a  is  expressed  in  the  b-frame).  Reference  frame  axes  will  be  defined  with  the 
axis  as  a  lower  case  letter  with  the  reference  frame  annotated  as  a  subscript  (e.g.,  Xf, 
defining  the  body  frame’s  x  axis) 

Relative  Motion:  relative  motion  between  two  different  frames  will  be  denoted  by 
the  subscripts  of  both  reference  frames,  read  from  right  to  left  (e.g.,  ccab  is  the  rotation 
rate  of  the  b-frame  relative  to  the  a-frame). 

Time:  time  will  be  identifiable  in  vectors  or  matrices  in  both  continuous  and  discrete 
time.  Continuous  time  will  be  designated  as  the  first  subscript  of  either  a  matrix, 
vector,  or  scalar  (e.g.,  a(t),  A(t)).  Discrete  time  will  be  designated  with  a  subscript 
of  the  time  index  (e.g.,  xt,Pi). 

2.2  Coordinate  Reference  Frames 

Navigation  is  fundamentally  based  around  the  concept  of  reference  frames.  Ref¬ 
erence  frames  define  a  convention  to  organize  and  account  for  all  necessary  physical 
motion  to  utilize  sensor  information.  A  multitude  of  reference  frames  have  been 
defined  over  the  years.  The  true  inertial  frame,  Earth-fixed  inertial  frame,  Earth  cen¬ 
tered  Earth  fixed  frame,  Earth-fixed  navigation  frame,  and  the  body  frame  typically 
encompass  the  reference  frames  used  in  inertial  navigation. 

True  Inertial  Frame. 

The  true  inertial  frame  is  a  theoretical  non-rotating,  non-accelerating  reference 
frame  where  Newton’s  laws  apply.  With  no  predefined  origin  or  orientation,  the  true 
inertial  frame  is  a  conceptual  reference  frame. 
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Earth-fixed  Inertial  Frame. 


The  Earth-fixed  inertial  frame  originates  at  the  center  of  the  earth  with  three 
non-rotating  orthogonal  axes  in  respect  to  the  stars.  Axis  z *  aligns  with  Earth’s 
polar  axis  and  x*  aligns  with  the  equator  and  points  to  the  first  star  of  Aries.  The  y j 
axis  is  the  complement  of  the  x*  and  z,  axes  maintaining  an  orthogonal  right  hand 
rule  coordinate  frame.  Properties  of  navigation  in  the  Earth-fixed  inertial  frame  are 
bound  by  Sir  Issac  Newton’s  Second  Law  of  Motion  F  =  m  ■  a.  Where  F  is  the  force 
with  units  N  =  Pp,  m  is  the  mass  of  the  body,  and  a  is  the  acceleration  of  the 
body.  Therefore,  physical  laws  of  motion  apply  and  proves  to  be  necessary  in  inertial 
navigation. 

Earth  Centered  Earth  Fixed  Frame. 

The  Earth  Centered  Earth  Fixed  frame’s  orthogonal  axes  are  fixed  based  on  the 
Earth.  The  ze  axis  aligns  with  Earth’s  polar  axis  like  the  Inertial  Frame,  and  the 
xe  axis  aligns  with  the  equator  and  0°  longitude.  Satisfying  orthogonal  properties 
and  the  right  hand  rule,  the  ye  axis  also  passes  through  the  equatorial  plane  as  the 
orthogonal  complement  of  the  xe  and  ze  axes.  The  Earth  Centered  Earth  Fixed 
(ECEF)  frame  rotates  with  respect  to  the  inertial  frame  about  the  ze  axis  at  a  rate 
of  cuie  =  360°/23hr  56min. 
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Figure  1.  Earth  Centered  Earth  Fixed  Frame 


Earth-Fixed  Navigation  Frame. 

Also  known  as  the  local  level  frame,  the  Earth-fixed  navigation  frame  designates 
an  origin  at  the  location  of  the  vehicle  with  a  right  hand  rule  convention  with  the 
zn  axis  perpendicular  to  the  earths  surface.  Unlike  the  inertial  frame  and  the  ECEF 
frame,  the  navigation  frame  changes  as  vehicle  position  changes.  Navigating  with  the 
navigation  reference  frame  requires  knowledge  of  the  Earth  rotating.  The  transport 
rate  is  dependent  on  the  location  of  the  vehicle  with  respect  to  the  turn  rate  of  the 
Earth  centered  Earth  fixed  frame  (ejen)- 


Utilized  in  this  research  as  the  Earth-fixed  navigation  frame,  satisfying  the  right 
hand  rule,  is  an  East  -  North  -  Up  (ENU)  convention  which  can  be  seen  in  Figure  2. 


Figure  2.  East  North  Up  Navigation  Reference  Frame 

Body  Frame. 

The  body  frame  is  utilized  for  vehicle  measurements  and  is  dependent  on  the 
mounting  orientation  of  the  sensors  on  the  vehicle.  In  a  strapdown  INS  system,  the 
body  frame  measurements  will  need  to  be  transformed  to  the  selected  navigation 
reference  frame  to  for  correct  incorporation  of  sensor  measurements.  A  diagram  of 
the  ENU  body  frame  is  depicted  in  Figure  3  where  X5  is  the  axis  pointing  out  the 
front  of  the  aircraft  nose,  y&  orthogonally  points  out  the  left  wing,  and  Z5  is  the  up 
direction.  The  origin  of  this  coordinate  frame  is  the  location  of  the  sensor  cluster. 
If  the  sensor  cluster  is  not  co-located  at  the  center  of  gravity  of  the  aircraft,  the 
measurement  lever  arm  must  be  handled. 
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Figure  3.  Aircraft  Body  Frame 


Camera  Frame. 

An  E/0  system  is  used  specifically  in  this  research  but  essentially,  a  camera  is 
considered.  The  camera  frame  is  located  at  the  optical  center  of  the  camera  which  is 
rigidly  attached  to  the  body  of  the  aircraft  for  the  purposes  of  this  inertial  navigation 
research.  Therefore,  the  camera  frame  is  co-located  with  the  body  frame  and  no 
DCM  or  lever  arm  is  necessary  when  using  E/0  system  measurements.  Camera 
frame  coordinate  axes  are  defined  with  the  zc  pointing  straight  out  from  the  optical 
center  of  camera,  and  xc  pointing  up,  and  yc  pointing  to  the  left  from  the  vantage 
point  of  the  optical  center  to  the  scene.  The  camera  frame  is  where  the  projection  of 
the  image  in  the  scene  will  be  resolved  for  measurements. 
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Figure  4.  Camera  Frame 

2.3  Coordinate  Transformations 

Coordinate  transformations  allow  measurements  in  one  reference  frame  to  be  uti¬ 
lized  in  another  reference  frame.  Vector  transformation  operations  are  accomplished 
with  a  matrix  capturing  the  relationship  between  the  two  frames  of  reference.  The 
process  of  transforming  information  between  coordinate  frames  can  be  accomplished 
with  a  few  techniques.  Direction  Cosine  Matrices,  Quaternions,  and  Euler  angle  ro¬ 
tations  all  can  be  implemented  to  perform  a  transformation  of  information  between 
frames.  Each  have  different  implementations  along  with  performance  qualities. 

Euler  Angle  Rotations. 

Euler  angles  are  obtained  as  a  sequence  of  three  rotations  performed  in  a  specific 
order  on  an  individual  axis.  The  convention  of  rotations  is  (3,2,1)  which  indicates 
the  Erst  rotation  is  about  the  zh  axis,  the  second  rotation  about  the  axis  and  the 
last  rotation  about  the  xj,  axis.  Individual  rotations  about  each  specific  orthogonal 
axis  can  be  seen  in  Equation  (2.3).  Euler  angle  rotations  are  a  three  parameter 
transformation  with  singularities  existing  at  elevation  angles  ±90°  [21]. 
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Combining  the  individual  Euler  angle  axis  rotations  in  the  (3,2,1)  convention  yields 
the  DCM  which  rotates  the  body  frame  to  the  navigation  frame. 


C”  =  Ci  •  c2  •  c3  (1) 

Expanding  Equation  (1)  shows  the  relationship  between  each  body  frame  and  navi¬ 
gation  frame  axis.  The  full  C£  expanded  is 
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n 
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cos  9  cos  0  —  cos  0  sin  0  +  sin  0  sin  9  cos  0  sin  0  sin  0  +  cos  0  sin  9  cos  0 
cos  9  sin  0  cos  0  cos  0  +  sin  0  sin  9  sin  0  —  sin  0  cos  0  +  cos  0  sin  9  sin  0 

—  sin  9  sin  0  cos  9  cos  0  cos  9 

(2) 


where  each  element  in  the  matrix  represents  the  cosine  between  the  specific  coordinate 
frame  axes. 

Simplification  of  Equation  (2)  due  to  small  Euler  angles  is  validly  invoked  in  this 
research  due  to  wings  level,  heading  hold  flight,  and  an  East  direction  of  flight.  That 
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is,  (0,  9,  <p)T  =  small  angles.  Combining  each  Euler  angle  rotation  generates  a  DCM. 
Because  of  small  Euler  angle  assumptions,  the  expanded  Euler  angle  rotation  matrix, 
DCM,  necessary  to  rotate  the  body  frame  to  the  navigation  frame  becomes  Equa¬ 
tion  (3). 

1  —ip  9 

Cnb «  0  1  -0  (3) 

-9  0  1 

Direction  Cosine  Matrix. 

The  Direction  Cosine  Matrix  is  a  3  x  3  matrix  performing  four  parameter  trans¬ 
formations  which  eliminate  singularity  concerns.  Each  element  of  the  DCM  shown 
in  Equation  (4)  is  equal  to  the  cosine  of  the  angle  included  between  the  orthogonal 
basis  unit  vectors  of  the  original  reference  frame  and  the  target  reference  frame. 


axes  which  makes  up  an  element  of  the  DCM. 
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A 


Figure  5.  DCM  Element  Cosine  Between  Axes 


An  example  implementation  of  DCMs  can  be  seen  in  Equation  (5). 


aW  =  C-a(fe)  (5) 

Where  vector  a®  resides  in  the  body  frame  and  vector  resides  in  the  navigation 
frame.  The  DCM  C ^  rotates  the  information  in  vector  a^  to  the  navigation  reference 
frame  yielding  a^.  An  important  orthonormal  property  of  DCMs  exists: 


c;cf  =  cjc‘  =  i  (6) 

Direction  Cosine  Matricies  will  mainly  be  used  becuase  of  the  absence  of  singularities, 
compared  to  Euler  angle  rotations. 

Quaternions. 

Quaternions  are  also  a  four  parameter  coordinate  transformation  like  a  DCM 
transformation,  containing  no  singularities.  The  quaternion  q  is  a  quadruple,  shown 
in  Equation  (7)  where  the  four  components  are  referred  to  as  Euler’s  symmetric 
parameters  which  are  constrained  via  Equation  (8). 
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q  = 
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(7) 


ql  +  q22+ql  +  ql  =  l  (8) 

The  relationship  between  coordinate  frames  is  characterized  based  on  a  single 
rotation  about  a  vector  cj)  where  a,  (5, 7  are  the  angles  between  the  reference  axes  and 
the  axis  of  rotation  cj)  [21].  A  quaternion  rotation  vector  is  expressed  by  one  real  value 
and  three  imaginary  values  to  perform  the  transformation. 
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2.4  Inertial  Navigation 


(9) 


Inertial  navigation  systems  operate  within  the  laws  of  classical  physical  mechanics 
mathematically  describing  motion.  Given  technology  has  enabled  the  measurement 
of  the  normal  force  produced  from  a  body’s  acceleration  force,  mathematical  integra¬ 
tion  yields  a  calculation  of  changes  in  velocity  and  position  over  the  measurement 
frequency  of  the  sensor.  A  three-axis  strap  down  inertial  navigation  system  will  con¬ 
tain  a  minimum  of  three  orthogonally  mounted  accelerometers  measuring  specific 
force,  and  three  orthogonally  mounted  gyroscopes  measuring  angular  rate.  An  im¬ 
portant  concept  in  inertial  navigation  is  resolving  the  sensor  measurements  in  the 
navigation  frame  of  reference,  referred  to  as  mechanization. 
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The  self  contained  nature  of  an  inertial  navigation  system  increases  the  degree  of 
robustness  to  outside  interference,  compared  to  other  navigation  systems  dependent 
on  transmission  of  external  signals.  However,  the  challenges  associated  with  inertial 
navigation  systems  come  in  the  form  of  accumulation  of  errors.  Over  long  flight 
intervals  without  aiding,  INS  errors  can  render  the  INS  virtually  useless  for  precision 
navigation.  The  ability  to  model  the  errors  of  the  INS  become  paramount  when  using 
an  external  sensor,  like  an  E/O  system,  to  aid  an  INS. 

Accelerometers. 

Accelerometers  measure  translational  motion  with  respect  to  inertial  space.  In 
the  past,  accelerometers  were  strictly  mechanical  devices.  Recent  rapid  development 
of  Micro-Electrical  Mechanical  Sensors  (MEMS)  technology  has  yet  to  show  similar 
accuracy  and  precision  as  the  mechanical  accelerometers  for  use  in  navigation  quality 
inertial  systems.  But,  is  certainly  a  growing  field  of  research  and  has  made  significant 
developments  in  the  consumer  grade  inertial.  The  cost  associated  with  MEMS  devices 
makes  them  advantageous  in  situations  where  degrees  of  navigation  precision  can 
be  sacrificed,  but  the  majority  of  applications  use  MEMS  accelerometers  and  rate 
gyroscopes  for  control  system  feedback  rather  than  navigation. 

Measurements  from  an  accelerometer  measure  total  or  specific  force  which  includes 
a  gravitational  component,  not  just  the  acceleration  of  the  body.  A  breakdown  of  Sir 
Issac  Newton’s  Second  Law  of  Motion  in  terms  of  what  an  accelerometer  measures 
is  shown  in  Equation  (10),  where  g  is  the  force  due  to  gravity,  a  is  the  acceleration 
force  of  the  body,  and  m  is  the  mass  of  the  body. 


F  =  ma 
F  =  m(a  +  g) 


(10) 
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Therefore,  the  gravitational  component  must  be  subtracted  out  of  accelerometer 
measurements.  Because  gravity  is  not  uniform  all  over  the  Earth,  physical  position 
must  be  used  to  calculate  the  force  of  gravity.  Gravity  models  can  be  used  to  calculate 
the  force  of  gravity  at  a  physical  location  on  Earth  [5] .  After  the  gravity  is  subtracted 
out  from  the  specific  force,  the  remaining  force  less  the  gravity  model  errors  is  the 
acceleration  of  the  body. 

A  simple  accelerometer  model  is  shown  in  Figure  6  where  an  acceleration  along  the 
sensitive  axis  will  cause  the  proof  mass  to  move  in  the  opposite  direction,  measuring 
specific  force. 


Figure  6.  Simple  Accelerometer  Diagram 


Gyroscopes. 

Navigation  gyroscopic  sensors  are  used  to  measure  changes  in  angular  rate  in  order 
to  resolve  attitude  in  the  navigation  reference  frame.  Gyroscopes  are  critical  to  the 
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determination  of  the  body  to  navigation  frame  coordinate  transformation.  Figure  7 
shows  a  diagram  of  a  2-axis  gimbaled  gyro,  which  is  different  than  a  strapdown  system 
but  conceptually  derives  the  INS  error  equations  the  same. 


Inner  Gimbal 
Axis 


Axis 

Figure  7.  Two- Axis  Gyroscope  Schematic  Diagram 

Gyroscope  errors  come  in  the  form  of  drift  of  the  spin  axis  which  occurs  over 
time.  Hence  long  navigation  scenarios  will  experience  increased  gyro  drift  and  require 
correction. 

Strapdown  Inertial  Navigation. 

Strapdown  Inertial  Navigation  refers  to  inertial  systems  rigidly  attached  to  the 
body  of  the  vehicle  as  compared  to  a  space  stabilized  system  mechanically  isolating  the 
INS  from  body  motion  via  gimbals.  Drivers  for  strapdown  system  use  mainly  include 
size,  weight,  and  cost.  The  complexity  of  space  stabilized  systems  are  directly  related 
to  the  size  and  weight  of  the  system  as  well  as  an  increase  in  cost.  As  mentioned, 
mechanization  is  the  process  of  resolving  the  navigation  state  information  in  the 
navigation  frame  of  reference.  Information  about  the  navigation  frame  of  reference 
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and  how  it  changes  over  time  must  be  known.  For  example,  the  Earths  rotation 
must  be  taken  into  account  when  mechanizing  from  the  body  frame  to  the  navigation 
frame. 

INS  Error  Sources. 

The  INS  inertial  instrument  cluster  made  up  accelerometers  and  gyroscopes  are 
susceptible  to  errors  which  can  be  calculated  and  modeled  to  enhance  performance  of 
the  INS  navigation  solution.  Errors  will  be  presented  with  respect  to  each  sensor,  a 
section  for  accelerometers  and  a  section  for  gyroscopes.  Although  some  of  the  error 
sources  may  be  modeled  similarly,  there  is  a  distinct  difference  in  magnitude  for  the 
errors  in  the  accelerometers  and  gyroscopes. 

Accelerometer  Errors. 

Accelerometer  error  sources  covered  in  this  section,  specifically  apply  to  the  strap- 
down  INS  models  as  there  are  many  different  types  of  accelerometers  which  have 
specific  errors  of  their  own  [21]  [23]. 

•  Bias  -  An  error  source  displacing  the  measurement  with  a  fixed  value  or  slow 
varying  additive  error.  Bias  errors  can  be  determine  with  inspection  of  the 
output  with  zero  acceleration  input  on  the  sensitive  axis. 

•  Scale  Factor  -  Errors  in  the  conversion  from  output  signal  measurements  to 
input  acceleration  by  a  scalar  quantity.  Where  the  scale  factor  must  be  applied 
to  the  output  signal  measurement  to  determine  the  input  acceleration  of  the 
body. 

•  Sensor  Misalignment  -  Sensor  misalignment  encompasses  the  manufacturing 
and  installation  of  the  accelerometer  sensors.  The  assumption  is  the  sensors  are 
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all  mounted  perfectly  orthogonal  which  is  physically  not  practical,  leading  to 
sensitive  axis  measurement  errors. 


•  Vibration  -  External  motion  vibrations  can  cause  an  additional  bias  error 
correlated  with  the  sensor  vibration  environment. 

•  Measurement  Noise  -  Originating  from  the  components  used  in  manufac¬ 
turing  of  the  sensor,  measurement  noise  encompasses  electrical  noise,  thermal 
noise,  and  any  other  device  level  noise  sources  associated  with  defining  the  noise 
level  under  which  a  signal  cannot  be  detected. 

•  Gravity  Model  -  Accelerometers  measure  specific  force  containing  the  local 
gravity  component,  which  must  be  subtracted  out  to  obtain  the  acceleration  of 
the  body.  Local  gravity  models  are  calculated  based  on  position.  Position  is 
calculated  via  integration  of  accelerometer  outputs  which  has  error  components. 
Using  a  position  solution  corrupted  with  errors  to  subtract  the  acceleration  due 
to  gravity  from  the  accelerometer  output  is  the  source  of  the  gravity  model 
error. 

Gyroscope  Errors. 

Gyroscope  error  sources  covered  in  this  section  limit  the  accuracy  of  the  Gyro¬ 
scope’s  measurement  of  angular  rate.  Similar  to  the  accelerometer  error  sources, 
techniques  for  modeling  some  of  the  error  sources  does  exist  [21]  [23]. 

•  Fixed  Bias  (^-independent)  -  An  error  source  observed  at  the  sensor  output 
when  no  input  angular  rotation.  This  bias  is  typically  a  random  constant  bias 
or  contains  slow  varying  additive  error. 

•  Acceleration  Dependent  Bias  (g-dependent)  -  Proportional  to  the  accel¬ 
eration  force  acting  on  the  sensor  which  impacts  not  only  the  sensitive  axis  of 
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the  sensor  but  orthogonal  axes  as  well.  Orthogonal  axes  are  impacted  because 
of  the  mass  unbalance  caused  by  the  acceleration  force. 

•  Anisoelastic  Bias  -  Errors  associated  with  acceleration  forces  present  along 
orthogonal  pairs  of  axes  with  respect  to  the  sensative  axis. 

•  Scale  Factor  -  Errors  in  the  conversion  from  output  signal  measurements  to 
input  angular  rates  by  way  of  a  scale  factor  or  proportional  quantity.  The  input 
angular  rate  is  the  desired  measurement  quantity. 

•  Sensor  Misalignment  -  Sensor  misalignment  refers  to  the  mechanincal  man¬ 
ufacturing  and  installation  of  the  gyroscopes.  It  is  assumed  the  gyroscopes  are 
mounted  in  an  orthogonal  orientation  which  in  physical  implementation  is  not 
true  leading  to  sensitive  axis  measurement  errors. 

•  Measurement  Noise  -  Originating  from  the  components  used  in  manufac¬ 
turing  of  the  sensor,  measurement  noise  encompasses  electrical  noise,  thermal 
noise,  and  any  other  noise  sources  associated  with  defining  the  noise  level  under 
which  a  signal  cannot  be  detected. 

INS  Error  Equations. 

The  dominate  INS  error  sources,  described  in  Section  2.4,  drive  the  direct-state 
dynamics  model  at  frequencies  dependent  on  relative  motion  [3].  Therefore,  an  error 
model  can  be  applied,  encapsulating  the  INS  error  which  removes  the  high  frequency 
components  of  direct-state  modeling.  The  INS  error  equations  are  developed  to  model 
the  errors  in  Position  Velocity  Attitude  (PVA). 

Attitude  errors  are  characterized  as  the  angular  error  seen  in  each  axis  of  the 
navigation  frame.  The  attitude  error  equation  yields  a  differential  equation  express¬ 
ing  the  change  in  attitude  errors  over  time  [21].  Equation  (11)  shows  the  attitude 
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error  dependency  on  the  gyroscope  biases  Suj^\  and  the  movement  of  the  navigation 
reference  frame  in  inertial  space. 


w «  x  4,  +  -  c  (ii) 

Velocity  error  equation  dynamics  are  expressed  as  Equation  (12).  Where  the 
Coriolis  effect  comes  into  play  due  to  movement  in  a  rotating  frame,  Earth.  Error  in 
the  gravity  model,  hg  accounts  for  the  use  of  the  calculated  position  for  the  gravity 
model.  Errors  in  the  accelerometer  biases,  are  rotated  from  the  body  frame  to 
the  navigation  frame  using  .  The  skew  symmetric  form  of  the  forces  observed  by 
the  accelerometers  is  [f(n^x]  which  is  multiplied  by  the  error  in  angles  if). 


(12) 


^(n)  =  [f(n)x]?/>  +  C£5f(ft)  -  (2c 4n)  +  w£?)  x  hv 
-  {28  +  Swg?)  x  v  -  hg 

The  rate  of  change  in  position  error  is  defined  as  the  error  in  velocity  hvln) 


<Sp(")  =  <5v(n)  (13) 

Use  of  the  full  INS  error  model  can  bring  about  numerical  issues  and  induce  more 
error  into  the  system  due  to  modeling  error.  Hence,  simplification  of  the  INS  error 
model  becomes  advantageous  and  has  been  done  while  performing  research  and  in 
industry  [17]  [15]  [18]  [22]  [13], 

2.5  Driftmeter 

Dating  back  to  World  War  II  bomb  targeting,  navigators  on  aircraft  like  the  B- 
17  and  B-24  operated  a  navigation  device  known  as  the  driftmeter.  At  the  time, 
the  driftmeter  was  a  measurement  of  wind  drift  that  allowed  the  pilot  to  adjust 
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trajectories  to  remain  on  course  while  also  allowing  the  navigator  to  calculate  the 
ground  speed  for  dead  reckoning  navigation.  The  use  of  a  drift  meter,  also  known  as 
a  drift  indicator  or  drift  sight  was  the  only  navigation  information  tool  outside  of  radar 
ranges  from  beacons,  which  were  not  available  in  hostile  environments.  Navigators 
would  sit  in  the  glazed  nose  of  the  aircraft  and  use  an  to  optical  sighting  device  to 
track  arbitrary  ground  features  whenever  necessary,  like  in  Figure  8. 


Figure  8.  Navigator  Locating  Ground  Feaures  with  Driftmeter 


Many  models  of  the  driftmeter  were  designed  and  installed  for  operations  on  the 
B-17  and  B-24.  Figure  9  shows  a  typical  driftmeter.  Where  the  major  components 
are  the  eyepiece  the  navigator  would  look  through,  the  line  of  sight  handle  for  ground 
feature  tracking  along  the  longitudinal  reticle  lines,  and  reading  of  the  drift  scale 
indicating  the  drift  angle. 
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IX  EYEPIECE 


CAGING  KNOB 


Figure  9.  Typical  Driftmeter  [4] 


The  driftmeter  was  made  up  of  a  telescoping  optical  sight  attached  to  a  compass 
scale.  On  the  reticle  of  the  eyepiece,  multiple  grid  lines  were  etched  to  aid  the  naviga¬ 
tor  in  capturing  driftmeter  measurements.  Two  or  three  speed  lines  were  horizontally 
spaced  a  known  distance.  The  speed  lines  along  with  a  timing  device  were  used  for 
speed  measurements,  providing  a  ground  speed  measurement.  Tracking  a  stationary 
ground  feature  along  the  longitudinal  grid  lines  yielded  the  drift  angle  due  to  wind  as 
the  navigator  rotated  the  optical  sight  and  observed  the  compass  scale.  For  example, 
the  aircraft  flying  a  true  heading  of  100°  and  the  navigator  tracks  a  feature  by  turning 
the  sighting  device  to  the  left  20°  indicating  the  aircraft  is  drifting  20°  to  the  left  with 
a  ground  track  of  80°.  Figure  10  is  a  visualization  of  what  the  navigator  would  see 
looking  through  the  eyepiece.  By  convention,  the  angle  6  is  the  drift  angle  rather 
than  the  correction  angle [4], 
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2.6  Modeling 

Based  off  deterministic  modeling  techniques,  stochastic  modeling  incorporates  a 
noise  component  to  linear  systems  in  an  effort  to  compensate  for  deterministic  model 
short  comings.  Deterministic  modeling  does  not  account  for  certain  noise  sources 
that  can  be  observed  in  the  real  world,  which  is  where  stochastic  modeling  fills  the 
void.  Stochastic  models  account  for  mathematical  system  model  imperfections,  un¬ 
controllable  disturbances  impacting  the  system,  and  sensor  data  imperfections  [14]. 
A  brief  background  of  deterministic  modeling  will  be  presented  followed  by  stochas¬ 
tic  modeling  which  will  highlight  the  additional  fidelity  associated  with  stochastic 
modeling. 
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Deterministic  Modeling. 


Deterministic  system  modeling  provides  the  basis  for  mathematically  capturing 
the  physical  aspects  of  a  system.  Basic  time-domain  models  of  dynamic  systems  are 
done  in  the  state  space  form.  The  general  linear  state  space  equations  are  shown 
in  Equation  (14)  in  a  time  invariant  application  but  time  varying  is  also  acceptable. 
In  Equation  (14),  A  is  the  mathematical  dynamics  model  of  the  system,  x(t)  is  the 
state  vector,  T  is  the  input  matrix,  and  u(f)  is  the  input  vector.  The  output  of  the 
system  z(t)  is  formed  from  mapping  and  scaling  the  states  by  way  of  the  obersvation 
matrix  H.  Output  mapping  of  system  inputs  are  handled  by  the  D  matrix  which  is 
typically  zero  due  to  disturbances  attributing  for  most  inputs. 


x(t)  =  A  x(f)  +  Tu(f) 


(14) 


z(t)  =  Hx(f)  +  Du(f) 

For  a  linear  system  modeled  derministically,  there  exists  a  deterministic  solution  to 
Equation  (14)  which  is  defined  as  Equation  (15). 


x(t)  =  eA(*  to^x(f0)  +  f  e  A(t  r,ru(r)dr  (15) 

Jto 

Derivation  of  the  state  transition  matrix  $  is  achieved  through  an  exponential  func¬ 
tion  of  the  dynamics  matrix  in  respect  to  the  time  step  At  =  t  —  to.  The  state 
transition  matrix  is  used  to  propagate  the  state  vector  forward  in  time  based  on  the 
dynamics  model.  State  relationships  in  the  time  domain  are  captured  in  the  $  matrix 
which  will  be  used  later  to  explain  obersvability  and  correlation. 


eA(t  to)  ^ 


(16) 


Observability  of  a  system  is  defined  as  the  ability  to  calculate  the  state  based  solely 
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on  the  system  output  [11].  For  a  linear  time-invariant  system  expressed  in  state  space 
representation,  observability  can  be  tested  with  the  row  rank  of  the  observability 
matrix.  The  output  matrix  H  provides  a  mapping  of  the  states  to  the  output  while 
the  dynamics  matrix  A  captures  the  states  inter-relationships.  Equation  (17)  forms 
the  observability  matrix  which  test  for  system  observability. 


O(H.A) 


H 

HA 

HA2 


(17) 


HA""1 


For  a  system  to  be  completely  observable,  the  row  rank  of  the  observability  matrix 
must  equal  the  number  of  states  n.  Equation  (18)  shows  the  requirements  for  a  fully 
observable  linear  time-invariant  system. 


rank  O  —  n  (18) 

Deterministic  system  observability  from  a  theoretic  control  approach  has  been  in¬ 
vestigated  by  many  research  efforts,  but  [1]  provides  a  full  analysis  of  the  observability 
concerns  with  INS  navigation.  The  observability  connection  between  deterministic 
system  modeling  and  stochastic  system  modeling  will  be  explained  further  in  Chap¬ 
ter  III. 

Stochastic  Modeling. 

Referring  to  the  beginning  of  this  section  (Section  2.6),  stochastic  modeling  in¬ 
creases  the  fidelity  of  a  model  by  specifically  introducing  components  accounting  for 
model  imperfections,  uncontrollable  disturbances,  and  sensor  data  imperfections.  It 
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is  important  to  first  define  a  stochastic  process,  which  is  a  real  valued  function  a (t,  s ) 
where  t  is  a  time  set  and  s  is  a  set  of  the  sample  space  which  outputs  a  random 
variable  [14]. 

The  linear  system  stochastic  model  addressing  the  deterministic  model  gaps  is 
Equation  (19).  Similar  to  the  deterministic  model,  the  system  shown  is  time  invariant, 
but  the  framework  to  model  a  time  variant  system  exists.  Adding  to  the  deterministic 
model,  G  is  the  noise  input  matrix  which  maps  the  noise  realization  vector  w (t)  to 
the  states,  and  v(t)  which  is  the  measurement  noise  vector.  Adapting  the  system  to 
account  for  any  dynamics  modeling  error  is  accomplished  by  w (t),  while  v(f)  models 
real  world  sensor  errors  in  an  attempt  to  generate  more  realistic  sensor  measurements. 


x(t)  =  Ax(f)  +  ru(t)  +  Gw(t) 
z(t)  =  Hx(f)  +  v(f) 


(19) 


Noise  sources,  are  zero  mean,  white,  Gaussian  processes.  The  noise  sources  are 
defined  by  a  process  strength  or  covariance  matrix.  Process  strength  Q  is  used  to 
define  the  necessary  statistical  covariance  of  the  input  noise  while  similarly,  R  is  used 
to  define  the  strength  of  the  measurement  noise  covariance. 


E{w(t)  ■  w T{t  +  r)}  =  Q  (t)  ■  8(t) 


(20) 


E{v(t)  ■  vT(t  +  r)}  =  R(t)  •  5(t)  (21) 

In  stochastic  modeling,  the  states  composing  the  state  vector  x(f)  have  an  as¬ 
sociated  covariance.  The  covariance  defines  the  uncertainty  with  the  state.  An 
understanding  of  uncertainty  is  critical  in  navigation.  Where  position,  velocity,  or 
attitude  information  may  be  available,  but  is  not  useful  without  an  understanding  of 


the  uncertainty  of  the  available  information.  The  covariance  matrix  P^.  contains  the 
independent  covariances  of  each  state  as  well  as  the  cross-covariance  which  quantifies 
the  correlation  between  states.  For  example,  take  a  state  vector  in  Equation  (22) 
comprised  of  random  variables  (X,  Y,  ...etc.)  and  a  covariance  matrix  Equation  (23). 

X 
Y 

x(t)  = 

n 

-I  lxn 

2  2  2 
aXX  °X Y  '  '  '  aXn 

2  2  2 
aYX  aYY  aYn 

2  2  2 
anX  anY  ' ' '  ann 

Independent  covariance  values  for  a  random  variable  are  located  on  the  diagonal 
in  the  matrix,  representing  the  uncertainty  of  the  random  variable.  The  cross¬ 
terms  in  P^.  are  the  cross- covariance  between  two  separate  random  variables.  Cross¬ 
covariance  quantifies  a  correlated  relationship  between  random  variables.  Where  cor¬ 
relation  is  defined  as  a  measure  of  the  relational  strength  between  two  separate  ran¬ 
dom  variables. 

From  probability  theory  and  statistics,  the  state  vector  variables  are  modeled  as 
random  variables.  The  covariance  of  random  variables  can  be  established  using  the 
expected  value  operator.  Equation  (24)  shows  the  variance  relationship  in  probability 
and  statistics  used  to  populate  the  covariance  matrix. 

4F  =  s[(x-^)(r-M  (24) 


(22) 
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Where  //  is  the  mean  of  the  random  variable,  and  X  or  Y  is  the  random  variable. 


First  Order  Gauss  Markov  Process. 

A  First  Order  Gaussian  Markov  (FOGM)  process  [14]  takes  on  multiple  statistical 
properties  which  are  used  to  model  bias  behaviors  of  accelerometers  and  gyroscopes. 
First,  a  Markov  Process  is  characterized  by  the  Markov  Property  defined  as  memo¬ 
ryless.  Equation  (25)  mathematically  expresses  the  Markov  property  of  recursion. 


f(xk  |  xk^i  ...  xN )  =  f(xk  |  xk-i)  \/k  e  M,  k  -  1, ...,  N  <  k  e  M  (25) 


A  Gaussian  Process  can  be  characterized  simply  with  mean  and  variance  of  the 
process.  The  Normal  or  Gaussian  Probability  Distribution  Function  (PDF)  and 
Cumulative  Distribution  Function  (CDF)  are  well  known  to  be  Equation  (26). 


f(x,n,a) 


1  (g-M)2 
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Measurement  Models. 


(26) 


Measurement  models  are  used  to  take  physical  sensors  capable  of  measuring  phys¬ 
ical  space  to  the  simulation  space  by  way  of  mathematical  characterization.  The 
process  by  which  a  sensor  is  modeled  mathematically  is  extensively  documented  in 
[8]  and  [14]  while  also  being  verified  in  community  research. 

Specifically  in  the  research  presented,  two  measurement  models  will  be  introduced. 
A  bearing  or  angle  measurement  model,  and  a  linear  single  state  model.  The  angle 
measurement  model  serves  as  a  good  background  for  the  measurement  model  applied 
in  Chapter  III.  The  linear  single  state  model  is  used  in  Chapter  III  to  model  a 
barometer  altitude  aiding  sensor. 
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Angle  Measurement  Model. 


A  bearing  measurement  is  defined  as  the  angle  between  two  points  within  a  coordi¬ 
nate  frame  of  reference.  Information  about  the  direction  vector  connecting  the  points 
exists  with  the  absence  of  range.  With  knowledge  of  the  position  of  both  points, 
trigonometric  functions  enable  the  calculation  of  the  angle  between  the  points.  To 
illustrate  the  information  necessary  for  the  measurement  model  and  the  output  solu¬ 
tion,  an  example  is  presented.  Take  an  aircraft  flying  in  2-D  position  space  receiving 
angle  measurements  to  a  beacon,  referenced  as  a  target  (T)  located  at  (^ ). 

(*,y) 

h(x)  =  arctan(— — — ) 

Xj-  —  X 


Figure  11.  General  Angle  Measurement  Model 

Equation  (27)  shows  the  angle  measurement  equation  which  is  non-linear  and 
must  be  accounted  for  when  used  directly  in  Kalman  filtering. 

^(x)angie  =  arctan  (— — -)  (27) 

xt  —  x 

Therefore,  a  Jacobian  approximation  is  used  when  implementing  an  Extended 
Kalman  Filter  (EKF)  which  handles  the  non-liniearities  in  the  dynamics  and  mea¬ 
surement  model.  However,  there  is  still  a  degree  of  linearization  error  associated  with 
linearizing  the  measurement  model  [14].  The  Jacobian  approximation  for  the  angle 
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measurement  model  presented  in  Equation  (27)  is  shown  in  Equation  (28). 


dh(ic) angle  _  _ yT  -  y _ 

dx  (. xt  —  x)2  +  (dt  —  y )2 

dh(k) angle  _  X  -  XT 


(28) 


dy  (xT  -  x)2  +  (jjr  -  y )2 

Where  the  partial  derivative  of  the  measurement  equation  is  taken  with  respect  to  the 
state.  Other  linearizion  techniques  are  possible  when  manipulating  the  information 
captured  from  an  angular  measurement  and  will  be  presented  in  Chapter  111. 


Geometry  Based  Bearing  Measurements. 

Pure  bearing  measurements  provide  radial  information  while  the  range  is  unob¬ 
servable.  If  the  range  can  be  constrained  with  known  information,  then  the  bear¬ 
ing  measurement  becomes  a  much  more  information  rich  aiding  source.  In  the  case 
where  an  aircraft  is  flying  and  taking  bearing  measurements  to  features  located  on 
the  Earth’s  surface,  knowledge  of  elevation  data  or  even  use  of  a  barometric  altimeter 
could  be  used  to  constrain  the  elevation  of  the  ground  feature. 

The  E/O  system  provides  a  measurement  of  the  ground  feature  in  the  inverted 
image  plane  di  while  the  focal  distance  of  the  camera  is  known  /.  Altitude  of  the 
aircraft  h  is  typically  known  well  with  a  barometric  aiding  source  and  the  distance 
from  the  aircraft’s  ground  projection  to  the  ground  feature  is  d.  Invoking  a  similar 
triangle  relationship,  Equation  (29)  captures  the  proportional  relationship  between 
what  the  E/O  system  measures  and  the  aircraft’s  location  in  (x,z). 


di  d 

l  =  h 


(29) 


Derived  in  a  3-dimensional  case,  the  aircraft’s  true  position  and  the  true  posi¬ 
tion  of  the  ground  feature  maintain  a  relationship  through  geometry  [18].  Equa¬ 
tion  (30)  shows  the  geometry  relationship  between  the  true  aircraft  position  ( x,y,z ), 
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the  true  ground  feature  position  (xp,  yp,  zp),  and  the  measurements  from  the  E/O  sys¬ 
tem  ( Xf ,  yf).  The  focal  length  of  the  E/O  system  is  /,  C/  is  the  DCM  transformation 
from  the  body  frame  to  the  navigation  frame. 
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The  similar  triangle  relationship  can  be  explicitly  observed  in  the  term 

where  the  measurement  from  the  E/O  system  is  used  to  scale  the  Rlos  from  the 
aircraft  to  the  ground  feature.  Independently  separating  Equation  (30)  yields  three 


individual  measurement  equations: 


I R 


LOS  | 


X2f  +  X2f  +  f2 


1  0  0  c  /  Vf 


xf 


-f 


(31) 


yP  -  y 


I -Rlos  | 


x2f  +  x2f  +  p 


0  1  0  C/  vf 

-f 


(32) 


zp-  z  = 


I -Rlos  | 


xj  +  x2f  +  P 


0  0  1  C/  yf 


xf 


(33) 


Assuming  knowledge  of  ground  feature  elevation  information  zp,  manipulation 
through  substitution  can  constrain  the  Rlos  which  can  be  substituted  into  the  re¬ 
maining  two  equations  for  the  x  and  y  axes.  After  manipulation  of  Equation  (33), 
Equation  (34)  is  the  scaled  Rlos  which  can  be  substituted  into  Equations  (31)  and 
(32). 


\Rlos  I 

yjx2+y2f  +  p 


Zp-  Z 


0  0  1  c 


' b 


(34) 
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Referencing  back  to  Equations  (31)  and  (32),  Equation  (34)  can  be  substituted 
in  yielding  Equation  (35)  which  is  incorporating  the  knowledge  of  ground  feature 
elevation  data. 
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(35) 


Linear  Single  State  Measurement  Model. 


Use  of  a  simple  linear  state  model  is  applicable  when  modeling  relatively  linear 
measurement  sensors  containing  error  sources  that  can  be  characterized  with  biases  or 
Gaussian  white  noise.  Recall  Equation  (19)  where  the  measurement  model  is  linearly 
formed  by  mapping  the  state  vector  to  measurement  space  plus  white  Gaussian  noise. 
In  the  simple  2-D  space  position  model  used  in  Section  2.6,  a  barometric  sensor  in 
the  2-D  case  would  be  able  to  measure  the  Y  channel.  The  measurement  model  for 
the  two  state  system  example  would  be 


z  (t)  =  y  +  vk  (36) 

where  the  observation  matrix  would  map  the  y  state  to  measurement  space  and  add 
white  Gaussian  noise  account  for  sensor  errors.  This  linear  measurement  model  is 
useful  in  applications  where  deriving  a  linear  model  is  possible. 

2.7  Kalman  Filtering 

Kalman  filtering  serves  as  a  bedrock  of  navigation  estimation  Bayesian  Filtering. 
A  Kalman  filter  is  an  optimal  recursive  data  processing  algorithm,  specifically  the 
optimal  Minimum  Mean  Squared  Estimator  (MMSE)[14].  Implementing  a  KF  begins 
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with  with  initial  conditions,  dynamics  model,  measurement  model,  and  characteriza¬ 
tion  of  noise  sources.  Two  implementations  of  a  KF  with  respect  to  inertial  navigation 
are  widely  used,  direct  state  ( total  state  space),  and  indirect  state  ( error  state  space). 

Direct  State  Kalman  Filtering. 

Direct  state  Kalman  Filters  estimate  the  actual  states  such  as  vehicle  position, 
velocity,  and  attitude.  The  drawbacks  of  direct  state  Kalman  filtering  are  the  high 
sampling  rates  required  due  to  high  frequency  dynamics.  Therefore,  direct  state  KFs 
are  not  typically  used  in  real-time  navigation  applications.  Direct  mechanization  is 
typically  restricted  to  alignment,  calibrations,  and  bias  determination  in  a  testing 
environment  [14]. 


Figure  12.  Direct  State  Inertial  Navigation  Kalman  Filter  Block  Diagram 

Indirect  State  Kalman  Filtering. 

Indirect  state  Kalman  Filters  estimate  the  errors  in  a  particular  information  source 
compared  to  an  external  source.  In  the  case  of  inertial  navigation  research,  the  indi¬ 
rect  state  Kalman  filter  will  estimate  the  error  in  the  INS  using  the  difference  between 
the  INS  output  and  the  external  sensor.  In  the  case  of  vision  aided  navigation,  the  ex¬ 
ternal  sensor  is  an  optical  measurement  device  providing  information  to  the  Kalman 
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filter  via  images.  Tracking  of  features  over  time  and  their  movement  in  the  image 
plane,  with  the  assumption  the  ground  features  are  stationary,  generates  useful  infor¬ 
mation  the  Kalman  filter  can  use  to  update  the  state  vector.  High  frequency  dynamics 
are  captured  in  the  INS  sampling  due  to  the  robustness  of  the  error  model  while  the 
system  error  propagation  is  dominated  by  the  low  frequency  error  propagation  model. 

Figure  13  shows  an  indirect  state  Kalman  filter  with  a  feedforward  implementa¬ 
tion.  The  feedforward  is  executed  by  taking  the  output  of  the  Kalman  filter  error 
estimates  and  subtracting  the  error  estimate  from  the  INS  solution.  In  this  system 
model,  the  INS  is  completely  running  free  without  input  from  outside  correction 
sources.  Strictly  the  output  of  the  INS  is  corrected  rather  than  a  resetting  or  direct 
correction  to  the  inertial  system  which  has  pros  and  cons.  The  benefit  lies  in  the  self 
contained  nature  of  the  INS  which  will  still  be  able  to  operate  during  measurement 
outages  and  filter  unreliabilities.  However,  if  measurements  from  the  external  source 
become  unavailable,  the  navigation  solution  will  revert  to  the  free  running  INS  errors 
as  if  no  aiding  existed  from  the  external  source. 


Figure  13.  Indirect  State  Feedforward  Inertial  Navigation  Kalman  Filter  Block  Dia¬ 
gram 

An  indirect  state  feedback  filter  is  shown  in  Figure  14.  Compared  to  the  feedfor¬ 
ward  indirect  state  Kalman  filter,  the  feedback  filter  provides  direct  correction  to  the 
inertial  system.  This  method  of  filtering  maximizes  the  use  of  the  error  estimation 
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but  also  presents  system  level  concerns.  As  the  inertial  system  has  become  dependent 
on  the  Kalman  filter  and  the  measurements  from  the  external  sensor.  Erroneous  mea¬ 
surements  from  the  external  sensor  have  the  potential  to  corrupt  the  INS  without  the 
ability  to  revert  back  to  the  self  contained  INS  like  the  feedforward  implementation. 


Figure  14.  Indirect  State  Feedback  Inertial  Navigation  Kalman  Filter  Block  Diagram 

Kalman  Filter  Equations. 

The  Kalman  filter,  introduced  by  Rudlof  E.  Kalman  in  a  paper  titled  ”  A  New  Ap¬ 
proach  to  Linear  Filtering  and  Prediction  Problems”  initiated  the  connection  between 
the  state  estimate  conditioned  on  previous  measurements.  A  state-space  system  for¬ 
mulation  is  used  with  a  dynamics  model,  control  inputs,  and  measurement  model. 
As  a  recursive  algorithm,  the  Kalman  filter  uses  information  from  the  previous  time 
iteration  to  calculate  the  current  time  iteration  state  vector  and  covariance  matrix. 
Optimal  in  the  MMSE  sense,  an  estimate  of  the  state  vector  conditioned  on  all  mea¬ 
surements  available  is  realized;  shown  in  Equation  (37). 

xfc  =  E[xfc  |  Zfc]  (37) 

Operation  of  the  Kalman  filter  is  in  discrete  time,  but  at  each  discrete  time  step 
there  exists  two  instances  of  the  KF  estimate  x*,  and  the  covariance  matrix  P that 
approach  the  discrete  time  step  from  opposite  sides.  The  two  instances  of  the  estimate 
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and  covariance  stem  from  the  presence  of  an  update  at  time  k.  An  estimate  with  an 
update  x^  will  be  moved  forward  to  the  next  time  step  through  propagation  yielding 
Xfc+1.  Upon  update,  at  the  same  time  step,  the  KF  estimate  becomes  x£+1. 


The  Kalman  filter  can  be  implemented  as  an  online  estimation  algorithm  due  to 
the  recursive  nature  of  the  bayesian  estimation  filter.  Figure  16  shows  the  recursive 
nature  of  the  Kalman  filter  and  the  discrete-time  estimation  process. 


Figure  5.8  Kalman  filter  loop. 


Figure  16.  Indirect  State  Feedback  Inertial  Navigation  Kalman  Filter  Block 
Diagram[14] 
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Propagate. 


Time  propagation  in  the  Kalman  filter  projects  the  estimate  of  the  states  based 
on  the  dynamics  model  defined  forward  in  time.  Where  the  time  step  is  defined 
by  the  (t  —  t0 )  used  to  generate  the  discrete  time  state  transition  matrix  In 
vision  aided-INS  navigation  research,  the  additive  noise  sources  are  the  drift  and  bias 
associated  with  the  sensors  measuring  specific  force  and  angular  rate.  Propagation 
of  the  state  vector  is  accomplished  with  multiplication  of  the  state  transition  matrix, 
calculated  from  the  dynamics  model,  with  the  previous  state  vector  estimate.  The 
covariance  matrix  is  also  propagated  forward  in  time  with  the  state  transition  matrix 
along  with  additive  discrete  input  noise.  Kalman  filter  propagation  equaitons  are 
shown  in  Equation  (38).  Where  x)7  is  the  state  estimate  before  update,  denoted  with 
the  superscript  accent. 


Vi  =  <J>dx+ 

pk+i  =  *dPt*d+Qd 

Observation  Matrix. 


(38) 


The  observation  matrix  H^+i,  similar  to  the  deterministic  system  output  matrix, 
maps  the  states  to  measurement  space.  Modeling  the  KF  measurement  z^+i,  the 
observation  matrix  is  multiplied  by  the  states  and  the  measurement  noise  v^,+1  is 
added  to  the  resultant.  Equation  (39)  shows  the  modeling  of  KF  measurements  where 
Hfc+1  is  the  measurement  model,  or  matrix  for  a  linear  case,  and  vfc+1  is  the  noise 
associated  with  the  external  sensor  providing  update  information  to  the  Kalman  filter. 
Camera  noise  sources  will  be  discussed  further  in  the  digital  imaging  Section  2.8. 


z/c+i  —  Hfc+iXfc+i  +  vfc+1 


(39) 
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Update. 


State  estimate  updating  in  a  Kalman  filter  requires  a  measurement  z *.+1  to  be 
available  for  the  current  time  step,  referencing  Equation  (37).  If  a  measurement  is  not 
available,  no  update  step  is  executed  and  the  Kalman  filter  will  continue  propagating 
the  estimate  to  the  next  time  step.  When  a  measurement  is  available,  information 
from  an  external  sensor  will  be  used  to  update  the  current  estimates. 


^fc+i  —  +  Kfc+1(zfc+1  Hfc+1xfe+1) 


(40) 


Pfc+i  =  (I  —  K/c+1Hfc+1)Pfc+1 

Minimization  of  the  mean-squared  estimation  error  is  specifically  accomplished 
with  the  Kalman  gain  term  KA.+1  [8].  The  Kalman  gain  is  a  weighting  term  that 
scales  the  residual  z^+i  —  Hfc+ix^+1  based  on  the  covariance  matrix,  the  measurement 
model,  and  the  measurement  noise.  Filter  performance  can  be  specifically  tracked 
with  the  residual. 


KA:+1  =  p-+1H£+1(Hfc+1p-+1H£+1  +  R)-1 


(41) 


Residual. 

An  understanding  of  filter  performance  is  a  vital  metric  when  optimizing  a  filter  for 
a  specific  application.  Performance  of  the  filter  is  accomplished  with  comparing  the 
filter’s  estimate  of  the  measurement  versus  the  measurements  provided  by  the  external 
source.  The  comparison  of  the  filter  estimate  and  the  external  source  measurement 
is  know  as  the  residual.  Equation  (42)  shows  the  measurement  residual  and  residual 
covariance  which  are  a  performance  metrics  showing  how  the  filter  can  estimate  the 
measurements  from  the  external  source. 
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F/c+1  ^k+ 1 


(42) 


Sfc+i  —  Hfc+1Pfc+1H^+1  +  R 

2.8  Digital  Imaging 

A  digital  imaging  system,  specifically  a  camera,  takes  in  light  from  a  scene  through 
optics  which  are  detected,  amplified,  and  digitized  into  a  digital  output.  The  use  of  a 
digital  imaging  system  for  inertial  system  aiding  is  done  by  the  E/0  system.  Camera 
models,  noise  sources,  and  image  registration  topics  relevant  to  this  research  will  be 
explained  in  this  section. 

Camera  Models. 

An  illustration  of  an  image  system  model  is  shown  in  Figure  17.  A  scene  is 
created  from  the  reflection  of  light  off  objects  in  the  world.  The  scene  is  captured 
as  the  reflected  light  enters  the  optics  of  the  imaging  device.  Optics  change  the 
direction  of  the  light  via  a  convex  lens  to  focus  the  light  to  the  optical  center  of  the 
camera  and  project  the  inverse  scene  onto  the  detector.  Variations  in  thickness  and 
shape  of  the  optics  will  change  the  parameters  of  how  the  scene  is  projected  onto 
the  detector.  When  light  hits  the  detector,  photons  are  converted  to  charges,  and 
transferred  by  rows  in  a  raster  fashion.  The  voltage  signal  is  then  amplified  prior  to 
digitization  yielding  a  digital  output  of  the  scene  observed.  Analysis  of  the  digital 
output  from  the  imaging  system  enables  the  feature  tracking  done  in  image  aided 
inertial  navigation. 
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ILLUMINATION 


Figure  17.  Image  System  Model[23] 


Pinhole  Camera  Model. 

The  pinhole  camera  model  in  Figure  18  is  a  theoretical  concept  assuming  the  thin 
lens  aperture  approaches  zero.  As  the  thin  lens  aperture  approaches  zero,  light  from 
the  scene  will  be  focused  to  pass  through  the  aperture  at  the  exact  same  intersection 
point.  When  the  scene  is  projected  onto  the  image  frame  in  the  pinhole  model, 
the  scene  is  actually  inverted.  Projecting  the  inversion  of  the  image  frame  yields  a 
projection  of  the  scene  in  front  of  the  aperture  the  length  of  the  focal  distance. 


Scene 


< — 

- o— 

f - » 

Aperture 


Image 


Figure  18.  Pinhole  Camera  Model 
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Camera  Noise  Model. 


Multiple  sources  of  noise  are  present  in  Charge  Coupled  Device  (CCD)  camera 
images,  which  are  used  on  targeting  pods.  CCD  noise  sources  include  the  conversion 
of  optical  photons  to  electrical  signal  charges  and  the  probabilistic  measurement  error 
due  to  sampling.  Noise  associated  with  the  external  sensor  providing  update  infor¬ 
mation  to  the  Kalman  filter  is  critical.  The  associated  variance  of  the  measurements 
brought  into  the  filter  must  be  valid.  For  example,  if  the  measurements  brought  into 
the  Kalman  filter  have  an  incorrectly  associated  variance  that  is  smaller  than  the  true 
value,  the  Kalman  filter  will  incorrectly  trust  the  measurements  when  updating  the 
filter  estimate. 

Image  Registration. 

Although  image  registration  will  not  be  implemented  in  this  research,  computer 
vision  is  a  vital  component  to  image-aided  navigation.  Image  registration  brings 
about  issues  like  camera  calibration,  feature-point  matching  techniques,  and  compu¬ 
tational  burdens.  Camera  calibrations  are  used  to  capture  the  parameters  describing 
the  mapping  of  3-dimensional  scene  space  to  the  2-dimensional  image  plane  space. 
Theoretical  simulations  can  be  performed  with  the  pin  hole  camera  model,  but  in 
experimental  testing  the  distortion  caused  by  imperfections  in  the  camera  lens  must 
be  modeled  and  accounted  for  when  using  feature-point  matching  for  navigation.  A 
closed-form  solution  to  modeling  radial  distortion  [25]  was  developed  by  Zhang  which 
has  since  been  extended  and  improved  upon  for  use  in  camera  calibration  toolboxes. 

A  tutorial  on  Visual  Odometry,  specifically  Part  II:  Matching,  Robustness,  Op¬ 
timization,  and  Applications  dives  deep  into  explaining  the  current  applications  of 
Visual  Odometry  and  the  techniques  used  to  provide  relative  position  error  ranging 
from  0.1  to  2%  [19].  Fundamentally,  two  approaches  exist  to  track  features.  One, 
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identifying  features  in  one  image  and  subsequently  tracking  those  same  features’  prop¬ 
erties  throughout  future  images.  Two,  independently  detecting  features  in  images  and 
matching  the  features  based  on  similarity  characteristics.  Frandorfer  and  Scaramuzza 
present  a  review  of  the  main  feature-point  techniques  used  in  Visual  Odometry,  includ¬ 
ing  popular  blob  detectors  Scale  Invariant  Feature  Transformation  (SIFT),  Speeded 
Up  Robust  Transformation  (SURF),  and  Center  Surround  Extremas  (CENSURE). 

2.9  Related  Research 

There  is  no  shortage  of  research  on  aiding  an  INS.  With  the  emergence  of  GPS, 
the  accuracy  and  precision  provided  ideal  aiding  action  to  an  INS  for  navigation. 
However,  the  reliability  of  GPS  in  specific  operating  environments  has  ignited  the 
research  on  passive  sensors  to  aid  an  INS.  Specifically,  at  the  Air  Force  Institute 
of  Technology  (AFIT)  Autonomy  and  Navigation  Technology  (ANT)  Center,  vision- 
aided  inertial  systems  are  a  strong  research  thrust.  This  section  does  not  by  any 
means  represent  an  exhaustive  representation  of  the  communities  research  but  does 
highlight  the  research  used  to  motivate  and  guide  the  work  presented. 

Inertial  Navigation  System  Aiding  Using  Vision. 

James  Quarmyne  investigated  a  scripted  navigation  scenario  problem  similar  to 
the  research  presented,  where  two  ground  features  were  placed  in  the  held  of  view  of 
the  optical  sensor  used  to  aid  the  INS.  The  Simultaneous  Localization  and  Mapping 
(SLAM)  approach  was  used  to  constrain  the  growth  of  the  INS  errors  over  time. 
Augmenting  the  state  vector  with  the  geolocated  position  of  the  ground  features, 
along  with  the  covariances,  for  the  duration  of  the  flight  significantly  reduces  the  INS 
error  growth  [17]. 
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Fusion  of  Low-Cost  Imaging  and  Inertial  Sensors  for  Navigation. 

Mike  Veth  presented  a  fusion  of  low  cost  imaging  and  inertial  sensor  integration 
for  navigation.  Utilizing  a  tightly  coupled  system  structure  at  the  measurement  level, 
benehtting  from  the  reliability  of  the  inertial  system  over  short  time  periods  and  the 
duration  reliability  of  the  imaging  sensor  measurements.  Monte  Carlo  analysis  was 
done  to  simulate  the  indoor  profile  where  features  were  randomly  generated  on  walls  to 
formulate  expected  performance  of  the  integrated  system.  Experimental  testing  in  the 
hallways  of  AFIT  showed  that  using  the  tightly  coupled  system  along  with  stochastic 
feature  projection,  the  navigation  solution  error  was  significantly  constrained  [24], 

A  Covariance  Analysis  of  Vision- Aided  Inertial  Navigation:  3-D  Case. 

Relyea  and  Pachter  investigated  the  “bootstrapping”  method  for  INS  aiding  dur¬ 
ing  a  cross  country  flight  where  previously  identified  ground  features  do  not  reappear 
in  the  held  of  view.  The  transition  from  using  the  known  location  of  ground  features 
for  INS  updating  to  using  unknown  ground  features  for  INS  updating  is  covered. 
Where  the  system  is  augmented  with  the  position  estimate  and  covariance  associated 
with  the  ground  features  used  for  the  INS  aiding.  It  was  shown  that  the  growth  in 
the  errors  are  still  unbounded  but  the  rate  of  growth  was  significantly  reduced.  The 
volume  of  position  uncertainty  was  defined  for  an  unaided  INS  as  0.7071  km3  which 
was  reduced  to  7.4177  x  10-10  with  optical  tracking  aided  INS  where  ground  features 
were  successively  tracked  throughout  the  flight  [18]. 

INS  Aiding  Using  Passive  Bearings-Only  Measurements  Of  An  Un¬ 
known  Stationary  Ground  Object. 

Alec  Porter  constructed  three  main  scenarios  where  bearings-only  measurements 
were  used  to  aid  a  tactical-grade  INS  with  and  without  prior  information,  and  the 
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impact  of  bearings-only  measurements  at  varying  altitudes.  The  altitude  dependency 
was  evident  as  the  true  bearing  and  INS  corrupted  bearing  cannot  be  assumed  equal 
with  the  presence  of  significant  altitude  error.  His  analysis  showed  that  without  prior 
knowledge,  the  positional  accuracy  of  the  aircraft  is  minimally  aided.  Where  the 
bearing  measurements  used  are  more  effective  with  high  Signal-to-Noise  Ratio  (SNR) 
Line  of  Sight  (LOS)  measurements  in  a  low  Geometric  Dilution  of  Precision  (GDOP) 
flight  scenario  with  the  stationary  ground  feature. 
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III.  Methodology 


The  methodology  of  the  conducted  research  will  be  presented  as  a  step  by  step 
process  that  was  initially  equation  derived  and  then  validated  in  simulation.  First, 
an  understanding  of  what  World  War  11  navigators  were  able  to  provide  operating 
a  driftmeter.  Also,  defining  the  robustness  associated  with  the  INS  error  model 
due  to  maneuvering  flight  dynamics  versus  a  steady-state  flight  condition  will  be 
evaluated.  Then,  using  current  technology  to  generate  “driftmeter”  measurements 
for  INS  aiding.  A  wings  level,  constant  airspeed,  heading  and  altitude  hold  flight  is 
simulated  to  capture  what  a  contemporary  driftmeter  brings  to  the  table. 

When  working  with  non-dimensional  variables  and  parameters,  it  is  important  to 
calibrate  expected  performance.  The  INS  error  source  covered  in  this  research  will 
strictly  be  the  accelerometer  and  gyroscope  biases.  As  the  dominant  source  of  accu¬ 
mulating  INS  error,  it  makes  sense  to  begin  with  with  strictly  the  bias  error  sources 
to  test  performance.  Rather  than  a  plug  and  play  approach  with  known  specifications 
of  bias  error  a  values  provided  by  manufactures,  the  non-dimensional  cr  values  were 
simulated.  Calibrated  INS  bias  standard  deviations  were  used  to  propagate  the  simu¬ 
lated  navigation  state  INS  errors  that  would  be  experienced  during  a  flight.  Based  on 
the  INS  calculated  PVA,  determined  from  both  the  flight  trajectory  and  navigation 
state  error  realization,  simulated  bearing  angle  measurements  to  a  stationary  ground 
feature  are  calculated.  Initialization  of  the  Kalman  filter  is  a  very  important  facet 
of  the  INS  aiding  technique  presented  and  will  be  developed  further  in  the  section. 
At  the  conclusion  of  a  measurement  epoch,  the  INS  is  reset  by  subtracting  out  the 
Kalman  filter  navigation  state  error  estimate,  yielding  theoretically  a  more  accurate 
INS  calculated  PVA  solution.  The  purpose  of  collecting  measurements  throughout 
the  epoch  and  updating  at  the  end  is  to  gauge  the  strength  of  INS  aiding. 

Presentation  of  the  INS  aiding  methods  will  be  accomplished  in  two  stages.  Method 
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1  will  be  referred  to  as  Navigator  Replication  where  the  functions  of  the  navigator 
will  be  directly  mechanized  and  assumptions  are  made  about  what  the  navigator  had 
available.  Method  2  is  the  implementation  of  the  measurement  equation  derived  in 
Section  3.4  where  contemporary  technology  is  used  to  automate  the  functions  of  the 
navigator  and  the  driftmeter. 

3.1  Experiment 

The  experiment  of  this  vision-aided  inertial  navigation  research  lies  in  the  estima¬ 
tion  of  the  error  in  velocity  Svx  of  the  INS.  Tracking  a  ground  feature  throughout  a 
measurement  epoch  with  an  E/O  system  provides  information  about  the  INS  error 
in  velocity.  Based  on  the  INS  error  model  dynamics,  correlation  will  be  leveraged 
by  the  Kalman  filter  to  effectively  reduce  the  INS  error  growth  for  the  remainder  of 
the  flight  following  a  measurement  epoch.  If  possible,  estimating  a  portion  of  the 
accelerometer  bias,  which  is  a  major  source  of  INS  error  could  also  positively  impact 
the  resulting  INS  solution  following  a  measurement  epoch  [12]. 

3.2  Navigation  Scenario 

The  navigation  scenario  section  of  the  methodology  will  serve  as  a  baseline  of 
assumptions  and  conditions  set  for  the  vision-aided  inertial  navigation  research  sim¬ 
ulations.  Although  some  of  the  assumptions  and  simplifications  appear  to  make  the 
work  novel,  the  goal  is  to  determine  the  strength  of  INS  aiding  possible  with  a  vi¬ 
sion  sensor  if  a  ground  feature  is  tracked  manually  by  a  pilot.  Implementation  and 
real-world  testing  is  possible  with  this  research  as  the  next  logical  step  to  reduce  IP 
update  constraints. 
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Non-Dimensionalization. 


Variables  have  been  non-dimensionalized  via  substitution  to  eliminate  variable 
units  according  to  Table  1. 


Table  1.  Navigator  Driftmeter  Replication  Non-Dimensionaliztion 


Position 

Velocity 

Specific  Force  Error 

Angular  Rate  Error 

Time 

f 

vx  ^  — 

u,  v 

-►  'f 

5uW  _ *  hs^ 

V  VV 

vv  ->•  V 

Sfy  ->  •? 

So,®  -+  h5^ 

T  -y  T\ 

h 

z^f 

vz  ^  — 

6fm  v!‘> 

6uib)  -)•  hs“*b) 

*  V 

At  =  1 

Constant  aircraft  flight  simulation  parameters  used  to  non-dimensionalize  vari¬ 
ables  are  listed  in  Table  2: 

Table  2.  Steady-State  Flight  Constants 


h 

1000  m 

9 

10^ 

Sz 

V 

100^ 

s 

Non-dimensionalization  of  all  variables  and  parameters  simplify  calculations  and  pre¬ 
vent  errors  with  units.  Acceleration  due  to  gravity  is  taken  to  be  constant  and  rounded 
to  10  p|.  Equation  (43)  lays  out  the  non-dimensionalization  process  executed  for  all 
variables  in  Table  1. 


h  ■  g  _  1000  m  •  lOf  _  10000^ 
v2  ~  (100^)2  ~~  10000^  ~  1 


(43) 
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Flight  Conditions. 


Replication  of  the  navigator’s  operation  of  the  driftmeter  is  done  through  a  sim¬ 
ulated  1  hour  flight.  During  the  course  of  the  flight,  the  INS  errors  will  accumulate 
without  an  aiding  source.  Halfway  through  the  flight,  30  minutes,  a  ground  feature 
will  be  tracked  for  the  duration  of  an  epoch.  The  flight  is  broken  up  into  epochs  which 
are  10  time  steps  (seconds)  long  in  duration.  Therefore,  the  entire  flight  consists  of 
360  epochs.  Throughout  the  duration  of  the  flight,  the  aircraft  will  be  in  autopilot 
mode:  winds  level,  airspeed,  altitude,  and  heading  hold.  Theoretical  wings  level  flight 
conditions  indicate  no  attitude  changes  in  roll  and  pitch  (0,  6).  Due  to  external  forces 
like  wind,  wings  level  flight  assumes  small  attitude  changes  in  roll  and  pitch  exist  to 
maintain  wings  level  flight,  but  are  small  and  cancel  out  in  nature.  Airspeed  hold 
describes  a  constant  velocity  flight  condition  where  the  aircraft  is  not  accelerating  or 
decelerating.  Maintaining  altitude  simplifies  the  the  vertical  component  of  the  LOS 
vector  to  the  tracked  ground  feature.  Heading  hold  constrains  (?/?),  maintaining  a  con¬ 
stant  aircraft  direction  of  travel.  The  scripted  steady-state  flight  dynamics  eliminate 
analysis  ambiguities  so  the  aiding  action  associated  with  driftmeter  measurements 
can  be  understood. 

Measurement  Geometry. 

Geometry  of  the  navigation  scenario  and  measurement  arrangement  is  visualized 
in  Figure  19.  The  projection  of  the  aircraft  position  is  the  position  of  the  aircraft 
on  the  surface  of  the  Earth.  Denoted  with  the  variable  P ,  the  tracked  stationary 
ground  feature’s  height  is  assumed  known  via  elevation  data  or  calculation  with  the 
barometric  altimeter.  The  aircraft  body  frame  ( Xb,yb,Zb )  is  conveniently  in  the  same 
orientation  as  the  navigation  reference  frame  (xn,  yni  zn)  which  is  in  an  East-North- 
Lip  convention.  Aircraft  direction  of  travel  will  be  in  the  East  direction,  longitudinal 
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velocity  will  be  the  only  dynamics  induced  into  the  system.  The  camera  frame  of  the 
E/O  system  is  rigidly  attached  to  the  aircraft  and  co-located  with  the  body  frame  of 
the  aircraft. 


Figure  19.  Navigation  Geometry  of  Measurement  Arrangement 

Longitudinal  travel  in  the  xn  axis  will  cause  the  stationary  ground  feature  to  move 
through  the  inverted  image  plane  only  in  Xf.  Where  ( Xf,yf )  are  the  pixel  location 
of  the  ground  feature  in  the  inverted  image  plane.  The  inverted  image  plane  is  non- 
dimensionalized  with  the  focal  distance,  /,  and  is  therefore  lxl  non-dimensional. 
With  the  center  of  the  inverted  image  plane  defined  as  (0,0),  a  feature  entering  the 
held  of  view  of  the  E/O  system  will  be  at  ( )  =  (  °q5  ).  Where  the  value  of  a  ground 
feature  entering  the  E/O  system  is  positive  because  the  image  plane  is  inverted.  The 
measurement  equation  will  be  covered  in  Section  3.4. 

INS  Error  Model  -  Dynamics. 

Error  state  dynamics  are  formed  based  on  the  INS  error  equations  developed  in 
Section  2.4,  with  simplifications.  The  errors  propagated  forward  in  the  time  domain 
are  defined  as  the  true  navigation  state  minus  the  INS  calculated  navigation  state  in 
Equation  (44)  where  x  is  the  true  PVA  navigation  state  vector  and  xc  is  the  calculated 
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INS  navigation  solution  corrupted  with  5x  errors. 

5x  =  xc  —  x  (44) 

The  navigation  state  error  vector  is  defined  as 

T  i  t 

Sx=  Sp<-n\  5xl)  (45) 

where  <5p(n^  are  position  errors  in  the  navigation  frame,  <7vdn)  are  velocity  errors  in 
the  navigation  frame,  and  Sxj)  is  the  angular  error  vector  of  the  ENU  navigation  frame 
of  reference.  Accelerometer  and  gyroscope  biases  will  be  brought  into  the  system  as 
inputs  according  to 

<5U=  <5/f,  5fib\  5fzb\  5ux\  5u{y\  5u:{b)  (46) 

The  continuous  time  state  space  stochastic  model  is  expressed  in  a  linear  arrange¬ 
ment  which  is  driven  by  the  input  vector  and  white  Gaussian  noise  [14]: 

x(t)  =  A  x(t)  +  Tu(t)  +  Gw(i)  (47) 

The  state  transition  matrix  is  derived  from  the  continuous  time  dynamics  model 
of  error  propagation.  Rather  than  direct  state  modeling,  error  state  modeling  affords 
the  ability  to  capture  the  impact  of  high  frequency  dynamic  maneuvers  with  low 
frequency  sampling  rates.  The  continuous  time  model  for  PVA  used  in  this  research 
will  be  developed  next. 

Attitude  errors  are  characterized  as  the  angular  error  seen  in  each  axis  of  the  nav¬ 
igation  frame.  The  attitude  error  development  from  Section  2.4  yielded  a  differential 
equation  expressing  the  change  in  attitude  errors  over  time  [21] 
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The  flight  trajectory  defined  in  Section  3.2  allows  simplifications  to  the  attitude  error 
differential  equation.  The  constant  velocity  flight  path  along  the  xn  axis  and  navi¬ 
gation  over  a  flat  and  non-rotating  Earth  eliminates  the  error  in  the  turn  rate  of  the 
navigation  frame  5u^ .  Therefore,  the  change  in  angular  error  can  now  be  expressed 
as 


(48) 

For  small  Euler  angles  (if),  6 ,  0)  and  an  aligned  body  to  navigation  frame,  the  DCM 
performing  reference  frame  transoformations  from  the  body  frame  to  the  navigation 
frame  is 


c W,M)  = 


1  — 0  6 

0  1  — 0 

- e  0  i 


(49) 


Wings  level  constant  altitude  flight  conditions  dictate  the  nominal  =  I3  from 
which  the  perturbation  of  Cj*  can  be  solved 


0 

— <50 

56 

<50 

0 

— <50 

-56 

<50 

0 

(50) 


The  derivative  of  the  body  to  navigation  frame  DCM,  Cjj,  is  dependent  on  the  rate 
of  body  rotation,  and  the  rate  of  the  navigation  frame  rotation.  Due  to  no  body  rota¬ 
tions  in  a  wings  level  flight  scenario,  the  body  frame  and  navigation  frame  alignment 
remains  constant.  Reducing  Equation  (51)  to  the  constant  DCM  in  Equation  (2) 
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(51) 


Cn  _  pnr>(^)  (~\(n)  r^n 

b  —  *Lin  ^ b 

c?  =  o 

Misalignment  angles  of  a  notional  platform  system  which  is  mechanized  to  the 
navigation  frame  is  expressed  as 

SV  =  -5C^Cbn  (52) 

where  551/  is  the  skew  symmetric  form  of  the  angular  errors  vector  50,  ie., 


5^  =  50x  (53) 

Referring  to  Equation  (52),  the  angular  errors  can  be  calculated.  Keeping  in  mind, 
the  Euler  angles  of  the  aircraft  are  assumed  zero  in  the  nominal  C0 

0  —50  50  1  ip  —0 

55Ef  =  —  50  0  — <50  —0  1  0 

—50  <50  0  0  — 0  1 

:  J  L  J  (54) 

50  •  0  +  50  ■  0  —  <50  —  50  ■  (f>  —(50  •  0  +  50 

551/  =  —  <50  —  (50  •  0  <50  -0  +  50-0  —50  -0  —  50 

—50  —  5(j)  •  0  —50  -  0  +  50  50  ■  0  +  50  •  0 

Substituting  the  Euler  angle  values  from  the  nominal  C))  =  I3,  the  angular  errors 

matrix  5^  is 


5^ 


0 

50 

-50 

—50 

0 

50 

50 

—50 

0 

(55) 
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In  vector  form,  the  angular  errors  can  be  extracted  using  Equation  (53)  yielding 
Equation  (56).  Following  convention,  negative  angular  errors  is  not  regular  practice 
and  the  negative  will  be  relegated  to  the  other  multiplicand. 


5 


T 


— 5(j)  — 59  —Stjj 


(56) 


Velocity  error  equation  dynamics  are  expressed  referencing  Section  2.4.  Over  short 
flight  distances,  that  is,  during  an  INS  aiding  session,  the  velocity  error  equation 
dynamics  are  simplified  to  the  specific  force  vector  rotated  by  the  error  in  angles 
plus  the  effect  of  the  accelerometer  biases. 


6ir(n)  =  [f(n)x]^,  +  Cn6f(b)  _  +  x  £v 

-  (2 5u?  +  K -n)  X  V  -  hg 

Equation  (57)  shows  the  simplihed  velocity  error  dynamics  implemented  in  this  re¬ 
search  with  the  elimination  terms  accounting  for  the  rotation  of  the  Earth,  and  error 
in  gravity. 


5v(n)  =  [f(n)x]^  +  C^f(6)  (57) 

The  skew  symmetric  specific  force  matrix  is  derived  from  the  flight  conditions  in 
Section  3.2.  Constant  altitude,  winds  level  flight  along  the  xn  direction  generating  a 
specific  force  vector  with  only  gravitational  components  in  the  zn  axis  and  acceleration 
force  in  the  xn.  Theoretically,  no  forces  will  be  observed  in  the  yn  axis  in  the  developed 
navigation  scenario.  The  specific  force  vector  in  Equation  (58)  is  populated  according 
to  the  convention  of  body  forces  minus  the  gravitational  forces,  keeping  in  mind  the 
ENU  reference  frame  convention. 
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Therefore,  the  skew  symmetric  matrix  form  of  the  specific  force  vector  becomes 

0  -g  0 

f(n)x=F(n)=  g  Q  (59) 

0  ax  0 

The  rate  of  change  in  position  error  is  defined  as  the  error  in  velocity  8v^n\ 


hp(n)  =  5v(n)  (60) 

Referencing  Equation  (47),  the  navigation  state  error  dynamics  matrix  A  and  the 
input  matrix  T  in  Equation  (61)  are  formed  from  the  INS  error  equations  developed 
in  this  section. 

O3  I3  O3  03  O3 

8x=  o3  03  -F{n)  8x+  03  8u  (61) 

03  03  03  03  C£ 

Where  F('4  is  the  skew  symmetric  specific  force  matrix  from  Equation  (59)  and 
C£  =  I3  as  the  nominal  DCM  transformation  from  the  body  frame  to  the  navigation 
reference  frame. 

Due  to  the  presence  of  accelerometer  and  gyroscope  biases,  bias  error  states  are 
incorporated  into  the  system  by  way  of  augmentation.  The  sensor  bias  errors  are  mod¬ 
eled  as  Gaussian  distributed  random  variables  described  with  mean  (/1)  and  standard 
deviation  (a).  The  accelerometer  bias  errors  Sf  ^  and  the  gyroscope  bias  errors  8u^ 
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in  the  body  frame  are  described  as 


5f[b)  ~  N(0,  al  ) 

(62) 

6u®~N(  0,a2bg) 

The  navigation  state  error  vector  is  augmented  with  the  accelerometer  and  gyroscope 
biases  according  to 


5x 


<5v(n\ 


T 


6*l>,  5f(6),  Sw® 


as  well  as  the  dynamics  matrix  which  becomes  a  15  x  15  matrix. 


(63) 


A 


O3  I3  O3  O3  O3 

o3  o3  -vin)  cz  o3 

o3  o3  o3  o3  C'z 

o3  O3  O3  O3  O3 

O3  O3  O3  o3  O3 


(64) 


■-  -1  15x15 

Referring  back  to  the  state  space  stochastic  model,  Section  2.6,  the  noise  input  matrix 
G  is  formed  to  add  noise  accordingly.  In  the  case  of  a  constant  random  bias  error, 
no  input  noise  in  necessary. 


O3  O3 
Cb  03 


O3  O3 


O3  O3 


(65) 


■-  J  15x6 

Therefore,  the  state  space  stochastic  model  is  newly  defined  accounting  for  accelerom¬ 
eter  and  gyroscope  biases  by  way  of  augmentation.  Due  to  the  implementation  of 
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random  constant  biases,  the  inclusion  of  the  noise  input  matrix  and  noise  strength 
matrix  is  not  needed.  Modeling  of  biases  as  FOGM  processes  and  the  addition  of  INS 
drift  would  require  the  use  of  the  noise  input  matrix  and  noise  strength  matrix  [14]. 

Incorporation  of  a  barometric  altimeter  sensor  bias  is  incorporated  because  of 
the  sensitivity  of  the  z  channel  and  the  standard  accompaniment  of  an  altitude  aid¬ 
ing  sensor  with  a  navigation  quality  INS.  The  complete  state  space  system  model 
implemented  in  this  research  is 

O3  I3  O3  O3  O3  0 

03  03  -F(n)  C£  03  0  <5v(n) 

03  03  03  03  -cnb  0 

03  03  03  03  03  0  hfW 

03  03  03  03  03  0 

0  0  0  0  0  oj  [Sbi L 

The  initialized  covariance  matrix  associated  with  the  augmented  state  model 
shown  in  Equation  (67)  assumes  a  perfect  initial  INS  alignment  and  known  infor¬ 
mation  from  of  the  INS  bias  sources  from  the  manufacture  specifications. 

O3  O3  O3  O3  O3  0 

O3  03  03  03  03  0 

O3  O3  O3  O3  O3  0 

03  03  0  ala  °3  0 

03  03  03  03  0 

0  0  0  0  0  alb 
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3.3  Method  1:  Driftmeter  Replication 


This  section  will  outline  the  simulations  performed  to  validate  assumptions  made 
about  use  of  the  dritmeter.  Capturing  the  aiding  action  the  driftmeter  provided 
is  simulated  using  both  structured  steady-state  fight  dynamics  and  flight  dynamics 
with  acceleration  inputs  in  the  direction  of  travel.  The  measurement  model  is  derived 
as  linear.  Implementation  of  the  KF  used  to  replicate  the  navigator’s  driftmeter 
measurements  will  also  be  covered. 


INS  Error  Model  Robustness. 


With  a  simplified  wings  level  INS  error  model,  it  was  prudent  to  test  how  robust 
the  INS  error  model  was  to  dynamic  flight  conditions  rather  than  just  steady-state 
flight  conditions.  Therefore,  robustness  testing  of  the  INS  error  model  was  simulated 
to  determine  how  acceleration  forces  versus  constant  velocity  flight  impact  INS  error 
accumulation.  The  definition  of  the  error  state  model  indicates  that  acceleration  in 
the  direction  of  travel  will  yield  minute  differences  in  navigation  state  error  results 
but  the  investigation  is  warranted. 

Input  acceleration  forces  to  simulate  acceleration  or  deceleration  of  the  aircraft 
during  flight  in  the  xn  direction  changed  the  state  transition  matrix  3?^  from  being 
constant  to  becoming  time-dependent  <&dk+1-  Referencing  Equation  (58),  a  sine  wave 
function  was  used  to  generate  the  acceleration  and  deceleration  forces  to  drive  the 
flight  dynamics. 
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(68) 


The  magnitude  of  the  acceleration  force  input  ax  is  non-dimensionalized  with  the 
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force  of  gravity  g.  Acceleration  input  frequency  u  was  empirically  tested  to  determine 
the  the  highest  degree  of  INS  error  model  impact  as  high  frequency  repeating  inputs 
appeared  to  cancel  out  over  the  course  of  flight. 

Robustness  of  the  INS  error  model  in  the  direction  of  travel  will  be  simulated 
through  the  parameters  in  Table  3. 


Table  3.  Steady-State  Flight  Constants 


a 

9 

0.2 

0.005 

0.4 

0.05 

0.6 

0.5 

0.8 

1 

1.0 

Measurement  Model  -  Measurement  Equation. 

Referencing  Section  3.2,  the  measurement  model  for  World  War  II  navigator’s 
driftmeter  operations  will  be  constructed  in  this  section.  The  history  of  the  drift- 
meter  in  Section  2.5  outlines  the  measurements  the  navigator  used  to  calculate  the 
ground  speed  and  the  wind  vector.  Essentially,  the  navigator  was  able  to  get  a  veloc¬ 
ity  estimate  as  the  aircraft  was  on  autopilot  traveling  at  constant  airspeed.  With  a 
measurement  of  velocity,  the  navigator  could  determine  5v.  Therefore,  the  measure¬ 
ment  model  used  to  replicate  the  navigator’s  operation  of  the  dirftmeter  is  a  linear 
model  that  taps  5vx. 

Utilizing  the  barometric  sensor  to  stabilize  the  z  channel,  the  barometric  altimeter 
measurement  is  constructed  via  substitution  shown  in  Equation  (69).  Subtracting  the 
output  of  the  INS,  zc  from  the  output  of  the  barometric  sensor,  Zbaro  yields  the  error 
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in  the  z  channel. 


Z  Zc  Zbaro 

(69) 


Z  =  (Z  +  Sz)  -  (Z  +  Sbbaro ) 

Kalman  hlter  measurements  are  defined  as  the  difference  between  the  INS  velocity 
output  vXc  and  the  driftmeter  velocity  output  vXm ;  and  the  difference  between  the  the 
INS  altitude  output  zc  and  the  barometric  altimeter  sensor  output  Zbaro.  Equation  (70) 
shows  the  measurement  given  to  the  KF. 


Zfc+l 


( '  VXC  VXm  ^ 


(70) 


\^ZC  Zbaro  J 

By  substitution,  the  measurement  given  to  the  KF  can  be  expressed  in  error  terms. 
The  velocity  error  in  the  INS  is  Svx,  and  the  error  in  the  driftmeter  measurement  is 
due  to  the  optical  device,  Sxf.  The  INS  error  in  altitude  is  8z  and  the  error  in  the 
barometric  altimeter  measurement  is  Sbbaro,  from  Equation  (69). 


zfc+i 


/ 


SvT  —  2  5x 


\ 


s 


(71) 


^&6aro  J 

The  measurement  covariance  R,  the  uncertainty  associated  with  the  measure¬ 
ments  given  to  the  KF,  is  derived  as  Equation  (72).  Error  associated  with  the  track 
of  the  ground  feature  through  the  sighting  device  of  the  driftmeter  is  <jc.  No  human 
induced  timing  or  feature  tracking  error  is  considered. 


R  = 


a 


2 

baro 


+ 

o 


o 

2 

®baro 


(72) 


The  linear  measurement  matrix  for  the  measurement  given  to  the  KF,  H.  repli- 
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cates  the  navigator’s  driftmeter  measurements  and  the  use  of  a  barometric  altitude 
aiding  sensor.  Also  known  as  the  observation  matrix,  H  is  shown  in  Equation  (73). 


H 


000100000000000  -1 


(73) 


Using  the  measurement  model  developed  in  this  section,  the  use  of  a  Kalman  filter 
can  be  used  to  estimate  the  INS  error  in  velocity  and  altitude  during  a  measurement 
epoch.  Navigator’s  did  not  continuously  use  the  driftmeter  as  a  constant  measurement 
source.  Rather,  it  was  used  intermittently  for  course  correction  while  dead  reckoning 
when  operationally  available. 


Kalman  Filter. 

Use  of  the  Kalman  filter  will  provide  an  estimate  of  the  error  in  velocity  and 
error  in  altitude  which  can  be  used  to  correct  the  INS.  In  the  navigator’s  time,  error 
models  currently  used  as  community  standards  were  not  utilized  in  flight.  However, 
with  knowledge  of  the  INS  quality,  the  navigator  could  calculate  an  estimate  of  the 
error  covariance  using  flight  time.  Therefore,  a  comparison  will  be  drawn  between 
initializing  the  Kalman  filter  with  independent  covariance  terms  and  zeroing  out  all 
cross-covariance  terms  versus  initialization  with  the  full  f  ree  INS  covariance. 

In  the  scenario  when  the  Kalman  filter  is  initialized  with  the  independent  co- 
variances  and  no  cross-covariance  terms,  the  covariance  matrix  would  be  set  to 
Equation  (74).  With  only  knowledge  about  the  individual  decoupled  performance  of 
the  sensors,  a  time  propagation  of  the  uncertainty  is  possible. 
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P i(,V  0  0  0  0 

0  -.00  0 

Po  =  0  0  0  0  (74) 

0  00'-.  0 

0  0  0  0  P+  (/ree) 

L  u  U  U  U  rk(W  16)  J 

Utilizing  the  cross-covariance  terms  that  are  generated  because  of  the  state  tran¬ 
sition  matrix,  allows  measurements  of  a  state  to  affect  another  correlated  state. 
In  control  theory,  observability  indicates  whether  a  state  can  be  calculated  from  the 
output  [11].  State  correlation  from  cross-covariance  relationships  can  result  in  ob¬ 
servability  of  states  that  are  not  directly  incorporated  in  the  measurement  model. 
Initialization  of  the  Kalman  filter  with  the  cross-covariance  terms  results  in  correla¬ 
tion  of  position,  velocity,  and  accelerometer  bias  states  for  respective  axes.  The  initial 
covariance  matrix  with  correlation  is  Equation  (75). 

pj  =  p  t  {,r“)  <75) 

The  Kalman  filter  propagate,  update,  and  Kalman  gain  equations  are  show  in 
Equations  (76),  (77),  and  (78). 


£x++1  =  5xfc+1  +  Kfc+1(zfc+1  -  H<$xfc+1) 

Pfc+i  =  (I  —  Kfc+iH)P k+l  ,  k  —  0, 1, ...,  L  —  1 


(78) 
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At  the  end  of  a  measurement  epoch,  the  Kalman  filter  estimate  5x  will  be  used 
to  reset  the  INS  to  capture  the  resultant  aiding  action  native  to  the  inertial  system. 
The  INS  will  be  reset  by  subtracting  the  Kalman  filter  estimate  of  the  navigation 
state  error  and  setting  the  INS  error  covariance  to  the  Kalman  filter  covariance. 

<5xfc+1  =  5xfc+i  -  5x++1  (79) 


p£5  =  p2+i  (so) 

3.4  Method  2:  Contemporary  Driftmeter 

The  contemporary  driftmeter  is  the  implementation  of  the  driftmeter  measure¬ 
ments  the  navigator  used  but  accomplished  with  current  technology.  Use  of  a  more 
realistic  measurement  model  rather  than  a  theoretical  observation  matrix  translates 
the  information  available  from  an  INS  and  E/O  system  to  measurements  that  can 
be  used  by  the  Kalman  filter  to  estimate  the  navigation  state  error.  There  are  sim¬ 
ilar  elements  of  the  Navigator  Replication  simulation  development  that  exist  in  the 
Contemporary  Dritmeter  simulation. 

Measurement  Model  -  Measurement  Equation. 

A  measurement  epoch  is  the  time  the  aircraft  is  in  autopilot  mode  and  the  station¬ 
ary  ground  feature  is  tracked  by  the  pilot  while  in  the  E/O  system’s  held  of  view.  It 
is  during  a  measurement  epoch,  that  INS  aiding  action  will  take  place.  At  the  conclu¬ 
sion  of  the  INS  aiding  session,  the  INS  will  be  reset.  Figure  20  pictorially  shows  the 
geolocation  and  then  tracking  of  a  ground  feature  through  one  measurement  epoch. 
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Figure  20.  Pilot  Geolocating  and  Then  Tracking  a  Ground  Feature  ( P )  Through  a 
Measurement  Epoch 


A  relationship  of  similar  triangles  generated  from  Section  3.2  is  invoked  between 
the  image  location  in  the  inverted  image  plane  and  ground  feature  location  in  the 
reference  frame.  The  relationship  is  dependent  on  the  knowledge  of  both  the  E/O 
system’s  focal  length  /  and  ground  feature  elevation  -  see  Figure  19.  The  similar 
triangle  relationship  allows  the  construction  of  the  measurement  equation  shown  in 
Equation  (81)  [18]. 
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This  nonlinear  measurement  equation  ties  together  the  true  navigation  state,  the 
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“known”  position  of  the  ground  feature,  and  the  true  ground  feature  bearing  mea¬ 
surements  coming  from  the  E/O  system.  Equation  (81)  represents  two  independent 
measurements:  with  the  elevation  of  the  ground  feature  zp  known  from  terrain  data, 
the  three  measurement  equations  can  be  reduced  to  two  independent  nonlinear  mea¬ 
surement  equations  [16]: 
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(82) 


Equation  (82)  represents  two  nonlinear  measurement  equations  derived  from  Equa¬ 
tion  (81),  relating  the  true  aircraft  position  ( x,y,z ),  true  driftmeter  measurements 
(. Xf,yf ),  and  the  true  ground  feature  position  ( xp,yp,zp ). 

In  the  derivation  of  the  KF  equations  it  is  assumed  that  the  position  (xp,  ypi  zp) 
of  the  ground  feature  P  is  known.  The  latter  is  the  geolocated  position  of  the  ground 
feature  at  the  beginning  of  the  measurement  epoch,  which  is  calculated  also  using  the 
two  nonlinear  Equation  (82). 

The  two  nonlinear  measurement  equations  from  Equation  (82)  are  linearized  and 
used  to  derive  the  measurement  matrix  EC+i  used  in  the  KF.  Equation  (82)  is  also 
used  in  the  calculation  of  the  measurement  that  is  fed  to  the  KF.  The  measurement 
fed  to  the  KF,  z*,+i,  is  corrupted  with  INS  and  E/O  system  errors.  Specifically,  mea¬ 
surements  brought  into  the  Kalman  Filter  are  defined  as  the  difference  between  the 
measured  position  of  the  ground  feature  and  the  “ known ”  position  of  the  geolocated 
ground  feature.  The  KF  measurements  (z^+i)  are  the  difference  of  the  geolocated 
ground  features’  position  at  the  beginning  of  the  measurement  epoch  and  the  ground 
feature  position  calculated  using  subsequent  bearing  measurements  taken  throughout 
the  measurement  epoch.  This  embodies  the  optical  information  provided  by  a  drift- 
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meter,  and  is  given  to  the  KF  as  its  measurements,  z *.+1,  to  be  fused  with  the  inertial 
measurements. 

A  measurement  epoch  extends  through  the  discrete  time  instants  k  =  1, L.  At 
time  k  —  0,  before  the  measurement  epoch,  the  ground  feature  is  geolocated,  that 
is,  its  coordinates  (xPrn,  yPm)  are  calculated  according  to  Equation  (83).  The  true 
navigation  state  of  the  aircraft  on  the  Right  Hand  Side  (RHS)  of  Equation  (82)  is 
substituted  with  the  INS  output  which  is  corrupted  with  INS  errors.  The  INS  output 
is  designated  with  the  subscript  c.  Also,  on  the  RHS  of  Equation  (82),  the  true  image 
plane  pixel  location  (yff)  of  the  ground  feature  is  substituted  with  the  E/O  system 
measurement  of  the  ground  feature.  The  E/O  system  contains  measurement  errors, 
{&Syf)-  Measurements  from  the  E/O  system  are  designated  with  the  m  subscript. 
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The  “known”  coordinates  ( )  of  the  geolocated  ground  feature  are  used  throughout 
the  rest  of  the  measurement  epoch.  The  measurement  z^.+i  given  to  the  KF  is  the 
geolocated  ground  feature  throughout  the  epoch  at  time  k  =  1, ...,  L  generated  using 
the  calculated  (xc,yc,zc)  from  the  INS  and  x frn ,  y frn  provided  by  the  E/O  system 
minus  the  position  of  the  geolocated  ground  feature  at  time  l  =  0,  computed  using 
the  calculated  (xc,  yc,  zc)  also  from  the  INS,  and  Xfm,yfm  provided  by  the  E/O  system. 

Thus,  the  same  measurement  model  in  Equation  (83)  is  used  to  first  geolocate 
the  ground  feature  at  time  k  —  0  yielding  ( )  and  then  calculate  the  ground 
feature  position  at  time  k  =  1,2 ,...L  throughout  the  measurement  epoch  yielding 
( ypPl)k ■  In  summary,  calculated  values  are  provided  by  the  INS  and  are  denoted  by 
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the  subscript  c,  namely  (xc,  yc,  zc)  which  is  the  aircraft  position  containing  INS  errors 
(5p,  <5v,  <S\P).  The  measurements  from  the  E/O  system  are  (xfm,yfm),  corrupted  with 
Gaussian  distributed  pixel  location  errors  Sxf,5yf.  The  measurement  Zfc+1  given  to 
the  Kalman  Filter  is  displayed  in  Equation  (84). 

,k  =  l,...,L  (84) 

k= 0 

The  rationale  for  this  approach  is  as  follows.  For  the  purpose  of  deriving  the  KF, 
which  operates  at  time  k  =  1,  2, ...,  L,  the  position  of  the  ground  feature  on  the  Left 

Hand  Side  (LHS)  of  Equation  (82)  at  time  k  =  1,2...,L  is  taken  to  be  “known”.  At 

the  same  time,  the  position  (x,  y,  z )  of  the  aircraft  on  the  RHS  of  Equation  (82)  is 
perturbed: 

x  =  xc  —  Sx 

y  =  yc  —  by  (85) 

z  =  zc  —  Sz 

Following  the  convention  of  perturbing  the  position  of  the  aircraft  ( x,y,z ),  the 
body  to  navigation  frame  DCM  can  be  perturbed  according  to  Equation  (86) 


I  =  C/  =  (C?)c  -  SCI 


With  all  terms  on  the  RHS  of  the  equation  perturbed  and  now  expressed  in  error 


quantities,  subtraction  of  the  two  nonlinear  equations  made  from  Equation  (82)  yields 
a  linearized  measurement  equation. 


Linearization  of  the  Measurement  Equation. 
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Simplification  of  terms  in  Equation  (87)  yields  the  linearized  measurement  model, 
that  is,  the  observation  matrix  H/j+1  in  Equation  (88).  The  values  contained  in  the 
Hfc+i  matrix  are  the  true  nominal  variables  and  determined  by  the  autopilots  settings. 


The  time  dependent  observation  matrix  at  time  k,  which  can  be  augmented  with  O3X6 
to  account  for  the  accelerometers’  and  gyros’  biases  and  augmented  with  ( 0  0  -1  )T  to 
account  for  the  barometric  altimeter  sensor  random  constant  bias  is 
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The  3rd  row  in  the  Hfc+1  matrix  allows  for  the  inclusion  of  an  independent  altitude 
measurement,  in  this  case  a  barometric  altimeter  sensor  is  used.  The  measurement 
noise  variance  R  is  provided  in  Equation  (89)  and  is  determined  by  the  Gaussian 
distributed  noise  associated  with  the  pixel  location  of  a  feature  from  the  scene  in  a 
CCD  camera  [2],  The  covariance  matrix  of  the  measurement  error  needed  in  the  KF 
is 


R  = 


jj(zp-z)2  0  0 

0  yj(zp-z)2  0 

0  &baro 


(89) 


where  <Jbaro  is  the  barometric  altimeter  sensor  measurement  error  standard  deviation. 

The  time  dependent  H*.+1  matrix  and  the  R  matrix  define  the  linear  measurement 
model  relating  the  E/O  system  outputs  Xf  and  yj  to  the  INS  navigation  state  errors. 


Altitude  Aiding:  Barometric  Altimeter  Measurement. 

Incorporating  an  altitude  aiding  sensor  is  necessary  with  the  use  of  a  navigation 
grade  inertial  system  [10].  Typically,  the  altitude  aiding  sensor  of  choice  is  a  baromet¬ 
ric  altimeter  which  preserves  the  passive  nature  of  the  inertial  system  as  opposed  to 
a  radar  altimeter.  The  barometric  altimeter  will  measure  the  altitude  of  the  aircraft 
above  a  fixed  calibrated  level  which  is  provided  by  a  ground  based  system.  Because 
the  barometric  pressure  of  mercury  is  measured  to  the  hundredth  inch,  there  will  be 
a  bias  that  exists  in  the  barometric  altimeter  measurement.  Referencing  the  Hon¬ 
eywell  AM-250  [9],  with  accuracy  of  ±0.01  inHg,  1  Hg  at  0°  ~  2.67  m  at  0°.  This 
research  will  use  a  non-dimensionalized  barometric  altimeter  measurement  <Tf,aro  = 


The  barometric  altimeter  measurement  model  requires  the  addition  of  a  measure- 
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ment  bias.  The  incorporation  of  a  barometric  altimeter  bias  is  accomplished  as  a  con¬ 


stant  random  Gaussian  bias  also  non-dimensionalized  according  to  <Jbbaro  =  loocTm  = 
0.005.  The  sensor  measurement  model  is  defined  as  measuring  the  true  altitude  with 
the  inclusion  of  a  barometric  altimeter  bias. 


Zbaro  Z  T  $bbaro  (90) 

Deriving  the  altitude  update  measurement  Zbaro  that  is  used  by  the  Kalman  filter  to 
update  the  navigation  state  error  vector,  is  consistent  with  the  linearization  technique 
used  to  generate  the  E/O  system  measurements  for  the  Kalman  filter.  The  error  in 
altitude  when  incorporating  the  barometric  altimeter  will  be  the  INS  Sz  error  from 
the  zc  solution  minus  the  barometric  altimeter  sensor  bias,  Sbbaro- 

zc  =  z  +  Sz 

Z  =  Zc  —  Zbaro  (91) 

Z  =  (Z  +  Sz)  ~  (Z  +  Sbbaro) 

The  constant  linear  observation  matrix  H {baro)  is 


H  (baro) 


001000000000000-1 


(92) 


which  maps  the  navigation  state  error  vector  to  measurement  space. 

Use  of  the  barometric  altimeter  measurement  model  is  invoked  for  all  time.  As 
opposed  to  the  E/O  system  measurements  which  are  only  available  when  a  ground 
feature  is  in  the  field  of  view.  Therefore,  a  barometric  altimeter  sensor  measurement 
will  be  used  to  update  the  INS  at  every  time  step.  The  INS  error  in  altitude,  Sz, 
will  be  constrained  by  the  measurement  covariance  of  the  barometric  altitude  sensor. 
Correlation  will  also  enable  good  KF  estimates  of  Svz  and  Sf^  which  can  be  used  to 
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decrease  the  INS  error  Svz  and  8f^\ 

Kalman  Filter. 

The  barometric  altimeter  measurement  model  alone  will  be  used  for  all  time  with 
the  exception  of  the  epoch  when  ground  feature  measurements  are  available.  Prior 
to  an  INS  aiding  session  with  E/0  system  measurements,  the  observation  matrix  in 
Equation  (92)  is  linear  and  time  invariant.  When  E/0  system  measurements  are 
used  for  updating  during  the  epoch,  the  observation  matrix  becomes  the  linear  time 
variant  matrix  in  Equation  (88) 

Implementation  of  the  Kalman  filter  during  the  measurement  epoch  will  provide 
an  estimate  of  the  navigation  state  error  (5xfc+1  and  the  associated  error  covariance 
Pfc+i.  The  Kalman  filter  equations  used  in  Section  3.3  are  identical  except  for  the 
inclusion  of  a  time-dependent  observation  matrix  H^+i  during  the  measurement  epoch 
where  a  ground  feature  is  measured  with  the  E/O  system. 

Due  to  the  ground  feature  tracking  throughout  the  duration  of  the  measurement 
epoch,  a  time-dependent  observation  matrix  must  be  incorporated  into  the  KF  equa¬ 
tions.  The  navigation  state  error  estimate  x*,  is  propagated  forward  in  time  according 
to  Equation  (93). 


5xfc+1  =  <K5x+  ,  <5x+  =  0 

P/+1  =  <f>P+<hT  +  Qd  ,P+  =  P*  (free  INS) 

When  the  measurement  epoch  begins,  k  =  0,  the  ground  feature  is  geolocated  and 
no  bearing  measurements  are  taken  for  system  update.  Starting  at  time  k  =  1  and 
continuing  until  the  end  of  the  measurement  epoch  k  —  L,  bearing  measurements  to 
the  geolocated  ground  feature  are  used  to  update  the  navigation  state  error  estimate. 
The  Kalman  gain  is  computed  similar  to  Equation  (77),  but  now  calculated  with  the 
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time-dependent  observation  matrix  H/,;+1. 


K*+i  =  Pft+1H^+1(Hfc+1P^+1H^+1  +  R)-1  (94) 

The  Kalman  filter  estimate  and  covariance  using  measurements  up  to  time  k  +  1 
are  calculated  according  to  Equation  (95),  also  using  the  time-dependent  observation 
matrix 


—  hxfc+1  +  Hfc+ihxfc+1) 


(95) 


Pfc+i  =  (I  —  Kfc+iHfc+i )Pfc+1  ,  k  =  0, 1, L  —  1 
At  the  end  of  the  measurement  epoch  k  =  L,  the  estimate  and  covariance  com¬ 
puted  by  the  Kalman  filter  will  be  used  to  reset  the  INS,  by  subtracting  the  Kalman 
filter  estimate  from  the  output  of  the  INS  like  in  Equation  (79).  Keep  in  mind,  the 
navigation  state  error  estimates  are  only  subtracted  from  the  navigation  state  error. 
Specifically,  the  estimate  of  the  barometric  sensor  bias  is  not  included  in  the  INS 
resetting. 


<5xfc+i  =  5xfc+i  -  5x++1  (96) 

The  error  covariance  capturing  how  well  the  filter  has  estimated  the  states  is  car¬ 
ried  forward  and  used  to  reset  the  INS  error  covariance.  Resetting  the  INS  error  co- 
variance  is  executed  with  the  same  procedure  done  in  Section  3.3  with  Equation  (80). 

pfS  =  p  Ui  (97> 
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Navigation  State  Kalman  Filter. 


Reducing  the  number  of  states  in  the  Kalman  filter  increases  the  observability  in 
the  states  the  KF  does  estimate.  In  an  attempt  to  try  a  new  simple  technique  and 
move  away  from  potential  over  modeling,  a  navigation  state  Kalman  filter  is  imple¬ 
mented.  The  KF  state  vector  will  only  contain  the  navigation  state  error  (<5p,  Sv,  5tj)) 
and  the  barometric  altimeter  bias  ((ib&aro).  By  excluding  the  accelerometer  and  gy¬ 
roscope  biases  from  the  KF  state  vector,  observability  is  increased. 

Also,  fixing  the  position  error  when  entering  an  INS  aiding  session  and  tracking 
the  growth  in  the  velocity  error  throughout  the  measurement  epoch  is  key.  The  KF 
covariance  is  initialized  according  to  Equation  (98)  where  the  free  INS  computed 
covariance  is  used  for  all  states  except  for  position.  Fixing  <5p  at  the  beginning  of  an 
INS  aiding  session  allows  the  KF  to  estimate  the  total  error  in  velocity. 
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Therefore,  the  KF  <5p  covariance  will  be  set  to  zero.  The  concept  of  computing 
the  <5v  from  the  (ip  is  explained  in  Figure  21.  Examining  Figure  21,  the  measurement 
estimation  technique  used  corresponds  to  the  relationship  between  position,  velocity, 
the  geolocation  error  of  the  arbitrary  ground  feature,  and  the  growth  of  the  velocity 
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error  throughout  the  measurement  epoch.  The  estimate  of  the  error  in  position 
(civ  •  At)  growth  throughout  the  measurement  epoch  can  be  used  to  calculate  an 
estimate  of  the  total  error  in  velocity  <5v. 


Figure  21.  Calculated  5vx  Measurement  Concept 


Inclusion  of  the  navigation  state  KF  originated  due  to  the  measurements  the 
navigator  was  able  to  take.  The  benefit  of  using  driftmeter  like  measurements  over 
time  can  provide  an  estimate  of  the  total  velocity  error  rather  than  just  the  error  in 
velocity  that  has  increased  during  tracking  of  the  ground  feature. 
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IV.  Results 


Research  simulation  results  are  presented  in  the  following  order:  replication  of  the 
navigator’s  use  of  the  driftmeter,  replication  of  the  navigator’s  use  of  the  dirftmeter 
if  correlation  would  be  available,  contemporary  driftmeter  measurement  model  with 
the  ground  feature  known  and  unknown,  and  a  nav.  state  KF  calculating  Svx  from 

Spx- 


4.1  Navigator  Replication 


In  order  to  validate  the  approach,  a  replication  of  the  Navigations  performance 
with  the  driftmeter  was  constructed.  The  scripted  navigation  scenario  developed 
in  Section  3.2  was  implemented:  wings  level,  constant  airspeed,  altitude  and  head¬ 
ing  hold.  A  barometer  for  altitude  measurements,  E/O  system  measurements,  and 
knowledge  of  time  make  up  the  information  the  Navigator  used  to  calculate  a  velocity 
estimate.  Use  of  the  barometer  constrains  the  INS  altitude  error,  enabling  bearings 
only  measurements  of  the  ground  feature  to  be  taken  in  the  x  and  y  directions.  Keep 
in  mind  that  no  information  about  the  range  of  the  ground  feature  can  be  extracted 
from  the  bearings-only  measurements.  Referencing  Equation  (63),  the  states  for  the 
system  are  the  navigation  state  error  vector  augmented  with  the  biases  in  the  ac¬ 
celerometers,  gyroscopes,  and  the  barometric  altitude  sensor  bias.  The  replication  of 
driftmeter  performance  measurement  model  can  be  seen  in  Equation  (99),  where  the 
velocity  in  the  x  direction,  altitude,  and  baro  bias  are  observable. 


Hfc 


010000000000000  0 
001000000000000  -1 


(99) 


Figure  22  shows  the  results  of  the  simulation  with  tracking  of  a  ground  feature  at 
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half  time  (30  min)  into  the  flight  duration.  At  the  end  of  the  measurement  epoch,  the 
INS  is  reset  by  way  of  subtracting  the  Kalman  filter  navigation  state  error  estimates 
from  the  free  running  INS  navigation  state  measurements.  The  Kalman  filter  is 
initialized  with  the  free  INS  navigation  state  error  covariance  values,  ignoring  cross¬ 
correlation  terms  which  are  set  to  zero.  Knowing  the  ground  feature  is  stationary, 
tracking  it  throughout  the  duration  of  the  measurement  epoch,  the  filter  is  able  to 
estimate  the  velocity  error  from  the  system  output  through  observability.  However, 
errors  in  position  are  unobservable.  Conceptually,  accumulated  position  error  prior 
to  measurement  availability  and  estimation  update  cannot  be  corrected  purely  with 
a  relative  update. 


X  Position  Error 


Figure  22.  X  Pos,  Vel,  Accel  Bias  Error  -  Full  Channel  Navigator  Replication  (KF 
initialized  with  NO  cross-correlation) 

When  analyzing  the  observability  of  the  system,  rank  deficiency  indicates  the 
navigation  state  error  is  not  fully  observable  [11],  In  a  case  where  the  INS  error 
dynamics  are  known,  initializing  the  Kalman  filter  at  half  time  with  the  free  running 
INS  navigation  state  estimation  error  covariance  can  provide  observability  through 
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correlation.  Specifically,  P(|  of  the  Kalman  filter  is  set  to  from  the  free  INS.  Below, 
Figure  23  shows  the  results  using  observability  through  correlation. 


X  Position  Error 


Figure  23.  X  Pos,  Vel,  Accel  Bias  Error  -  Full  Channel  Navigator  Replication  (KF 
initialized  with  cross-correlation) 

As  opposed  to  the  Kalman  filter  initialized  with  zero  cross-correlation,  the  re¬ 
sults  using  correct  cross-correlation  yield  improvements  in  estimation  observability. 
Specifically,  x  position  and  x  accelerometer  bias  error  estimates  are  significantly  im¬ 
proved  compared  to  the  data  driven  estimates  obtained  without  observability  through 
correlation.  With  a  good  estimate  of  the  residual  accelerometer  bias  courtesy  of  cor¬ 
relation,  the  remaining  error  propagation  model  of  the  INS  error  will  be  significantly 
improved  as  the  accelerometer  bias  strongly  affects  the  velocity  and  position  error. 

Thus,  knowledge  of  the  INS  navigation  state  error  dynamics  allows  the  incorpora¬ 
tion  of  state  cross-covariance  based  on  the  INS  error  equation  propagation.  Initializing 
the  Kalman  filter  with  the  full  free  INS  covariance,  yields  a  x  position  estimate  coin¬ 
ciding  with  the  velocity  measurement  available  to  the  Kalman  filter.  At  the  conclusion 
of  the  1  hour  flight,  a  60%  reduction  in  error  covariance  is  realized  while  eliminating 
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a  large  portion  of  the  free  INS  error. 

Verified  as  a  proof  of  concept  in  simulation,  tasking  the  pilot  to  track  a  stationary 
ground  feature  can  provide  significant  position  error  improvements  (60%)  without 
knowing  the  position  of  the  ground  feature.  However,  it  is  important  to  note,  ob¬ 
servability  through  correlation  is  possible  because  the  flight  condition  prior  to  the 
measurement  epoch  is  known  and  in  this  case  it  is  the  same  flight  condition  during 
the  measurement  epoch. 

Implementing  the  measurement  model  developed  in  Section  3.3  demonstrates  a 
higher  fidelity  simulation  of  the  aiding  action  available  with  bearings-only  measure¬ 
ments.  Consider  the  automation  of  the  World  War  II  navigator’s  function  using 
contemporary  technology  to  significantly  reduce  the  human  and  instrumentation  er¬ 
rors. 

4.2  INS  Error  Model  Robustness  to  Axis  Acceleration 

The  robustness  of  the  INS  error  model  was  inspected  in  order  to  validate  the 
simplified  flight  conditions  pursued  in  Section  4.3.  Referring  to  Section  3.3,  the 
INS  error  model  was  tested  with  acceleration  inputs  in  the  ax  axis  prior  to  and 
following  and  INS  aiding  session.  During  the  INS  aiding  session  only,  wings  level 
flight  conditions  annotated  in  Section  3.2  are  used. 

Table  4  shows  the  difference  in  position  error  if  acceleration  inputs  prior  to  flight 
exist  vs  a  steady-state  flight  condition.  Taking  the  difference  between  the  resultant 
position  error  in  the  x  direction  at  the  end  of  the  1  hour  flight  was  used  as  a  robustness 
test  of  the  INS  error  model.  Equation  (100)  shows  the  difference  in  the  position  error 
in  the  x  direction  comparing  the  INS  error  model  with  steady-state  dynamics  and 
with  an  input  sinusoidal  acceleration  input. 
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(100) 


Ax  X  acceleration  %  steady— state 


The  values  are  non-dimensionalized  when  compared  and  therefore,  the  equivalent 
units  are  meters.  As  the  frequency  of  the  sine  wave  increases,  the  acceleration  inputs 
appear  to  cancel  out  over  the  course  of  the  flight  which  is  a  limitation  of  this  analysis. 
Hence,  the  empirical  testing  of  a  reasonable  frequency  to  simulate  an  acceleration 
flight  profile.  Even  with  increased  magnitude,  up  to  1  g  of  acceleration  force,  the 
largest  difference  in  final  position  error  does  not  exceed  20  m. 


Table  4.  INS  Position  Error  Difference  Ax  Results 


uirr) 

0.005 

0.05 

0.5 

1 

0.2 

4 

0.04 

-0.002 

0.004 

a 

0.4 

7 

0.07 

-0.006 

0.006 

9 

0.6 

11 

0.12 

-0.009 

0.008 

0.8 

15 

0.14 

-0.013 

0.012 

1.0 

19 

0.18 

-0.016 

0.015 

4.3  Contemporary  Driftmeter 

Consider  the  same  flight  condition  outlined  in  Section  3.2.  Simulations  were  ini¬ 
tially  run  for  a  case  where  the  position  of  the  ground  feature  is  known  at  the  be¬ 
ginning  of  the  aiding  session  to  validate  the  measurement  model.  Then  geolocation 
of  the  ground  feature  was  calculated  using  the  INS  and  E/O  system  measurements 
which  are  corrupted  with  <5x  from  the  INS  and  Sxf  and  Si/f  from  the  E/O  system. 

With  the  position  of  the  ground  feature  known,  the  filter  is  able  to  estimate  the 
INS  velocity  error  by  taking  measurements  over  time  to  a  stationary  ground  feature. 
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In  the  measurements  taken  throughout  the  duration  of  the  epoch,  the  ground  feature 
is  geolocated  with  geometry  using  the  INS  output  and  the  E/O  system.  Using  the 
known  location  of  the  ground  feature  is  similar  to  the  practices  of  IP  inertial  system 
updates  in  terms  of  an  absolute  update.  Testing  with  a  known  ground  feature  location 
given  to  the  KF  at  time  k  =  0  served  as  a  basis  for  validating  the  measurement  model. 


X10'4  X  Velocity  Error 


Figure  24.  X  Velocity  Error  -  Ground  Feature  Location  Known 

Figure  25  shows  the  geolocation  track  calculated  using  the  INS  and  the  E/O  sys¬ 
tem.  A  strong  relationship  between  the  altitude  error  Sz  and  the  ability  to  correctly 
geolocate  the  stationary  ground  feature  was  observed.  Geolocation  of  the  ground 
feature  was  done  eliminating  altitude  error,  eliminating  angular  error,  and  with  only 
the  E/O  system  error  for  comparison.  Also,  the  ground  feature  geolocation  through¬ 
out  the  epoch  is  distributed  around  the  initial  geolocation  due  to  the  accumulation 
of  errors  at  the  beginning  of  the  epoch.  The  drift  of  the  ground  feature  location 
throughout  the  epoch  is  attributed  to  the  increase  in  <5x,  St/),  and  the  E/O  system 
pixel  errors  observed  during  the  INS  aiding  session. 
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Gelocation  of  Ground  Feature  Through  Measurement  Epoch 


E  179.4 


Figure  25.  Ground  Feature  Geolocation  Track  -  Ground  Feature  Location  Known 


Geolocation  of  an  unknown  ground  feature  at  the  beginning  of  the  measurement 
epoch  proves  to  only  provide  an  estimate  of  the  accumulation  of  the  error  during  the 
INS  aiding  session.  This  is  due  to  the  presence  of  relatively  the  same  errors  used  to 
geolocate  the  ground  feature  at  l  =  0  and  throughout  the  epoch  at  l  =  1,2, 
Figure  26  shows  the  weak  estimation,  with  a  zoomed  in  view  in  Figure  27,  of  the 
total  5vx  since  the  beginning  of  the  flight. 
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X  Velocity  Error 


Figure  26.  X  Velocity  Error  -  Ground  Feature  Location  Unknown 


X10'4  X  Velocity  Error 


Figure  27.  X  Velocity  Error  -  Ground  Feature  Location  Unknown  -  Zoomed  In 


With  a  weak  estimate  in  velocity  error,  Sv,  the  estimation  of  the  accelerometer 
bias  is  proportionally  weak.  A  good  estimate  of  the  accelerometer  bias  would  have 
been  used  to  decrease  the  accumulation  of  INS  error  after  the  INS  aiding  session  in 
Figure  28. 
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X  Accel  Bias  Error 


Aided  INS  5  x 

-  KF  5  x  est 

-  Aided  INS  Error  a 
KF  a 

Free  INS  a 


1750  1760  1770  1780  1790  1800  1810  1820  1830 

Time  (s) 


Figure  28.  X  Accelerometer  Bias  Error  -  Ground  Feature  Location  Unknown  -  Zoomed 
In 


The  ground  track  of  the  geolocated  ground  feature  still  follows  the  relationship 
dependency  on  Sz.  Which  validates  the  use  of  the  barometric  sensor  for  altitude 
aiding  to  constrain  the  errors  in  the  z  channel. 


x  179.35 


Gelocation  of  Ground  Feature  Through  Measurement  Epoch 
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Figure  29.  Ground  Feature  Geolocation  Track  -  Ground  Feature  Location  Unknown 
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4.4  Navigation  State  Kalman  Filter 


The  navigation  state  KF  attempts  to  investigate  the  possibility  the  system  is  over 
modeled  for  the  measurements  available.  From  an  observability  perspective,  reduction 
of  augmented  correlated  states  will  enhance  the  observability  of  the  system.  Presented 
in  this  section  are  the  results  of  the  implementation  of  a  navigation  state  KF. 

The  results  show  that  an  estimate  in  velocity  does  more  than  just  estimate  the 
growth  of  velocity  error  during  the  measurement  epoch,  shown  in  Figure  30.  There¬ 
fore,  using  the  estimate  in  velocity  error,  the  INS  is  reset  which  in  turn  positively 
impacts  the  growth  of  error  in  position,  shown  in  Figure  31. 


Figure  30.  Reduced  State  KF  with  calculated  Svx  -  X  Velocity  Error 


Figure  31.  Reduced  State  KF  with  calculated  Svx  -  X  Position  Error 
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V.  Conclusion 


The  research  performed,  investigating  a  bearings-only  INS  aiding  method  with  an 
E/O  system  has  continued  the  push  for  inertial  navigation  research.  Reliance  on  GPS 
systems  is  becoming  an  obvious  concern  in  many  applications  including  the  military. 
Research  focusing  on  passive,  self  contained  navigation  sensors  is  paramount.  Navi¬ 
gators  in  World  War  two  were  not  afforded  the  technology  available  today,  but  were 
still  able  to  navigate  via  dead  reckoning  with  a  driftmeter.  Therefore,  the  investiga¬ 
tion  into  what  the  driftmeter  actually  brought  to  the  table  is  warranted.  Realizing 
the  human  and  component  errors  corrupting  the  navigator’s  use  of  the  driftmeter, 
the  question  this  research  focused  on  is  can  we  use  driftmeter  principles  with  con¬ 
temporary  technology  to  aid  a  navigation  grade  INS.  Simulations  were  developed 
to  replicate  the  navigator’s  use  of  the  driftmeter  during  the  course  of  a  1  hr  flight. 
The  results  were  evaluated  indicating  the  navigator  was  able  to  calculate  his  error 
in  velocity  well.  Understanding  the  relationship  between  position  and  velocity  pre¬ 
sented  a  segue  to  the  geolocation  of  an  arbitrary  ground  feature  at  the  beginning  of 
a  measurement  epoch  and  subsequently  geolocating  the  ground  feature  throughout 
the  epoch.  The  growth  of  the  error  throughout  the  epoch  can  be  used  to  back  out  an 
estimate  of  the  INS  error  in  velocity,  Sv. 

5.1  Conclusions  -  Contributions 

Simulation  results  presented  in  Chapter  IV  are  not  of  the  precision  navigation 
performance  of  a  consumer  grade  GPS  standard  positioning  source  [7].  But  the  value 
added  comes  in  the  form  of  the  ability  to  generate  an  estimate  of  the  total  error  in 
velocity  positively  impacts  the  remaining  growth  of  the  INS  errors.  The  ability  to 
implement  this  technique  with  current  operational  technology  with  little  integration 


makes  the  approach  advantageous  for  flight  testing. 


5.2  Recommendations  for  Future  Work 

A  robust  analysis  on  the  impact  of  trajectory  on  the  INS  errors  to  validate  the 
simplified  error  equation  model  used  to  propagate  the  free  INS  would  be  benefi¬ 
cial  from  a  verification  standpoint.  Even  though  the  purpose  of  error  models  are 
to  eliminate  the  dependency  on  high  frequency  component  dynamics,  the  modeling 
error  associated  with  dynamic  flight  conditions  while  running  steady-state  INS  error 
equations  should  be  tested. 

It  was  discovered  the  high  dependency  the  z  channel  had  on  the  geolocation  of 
the  ground  feature  due  to  geometry.  Understanding  the  GDOP  constraints  in  the 
flight  scenario  and  the  operating  limitations  of  this  approach  would  be  valuable.  The 
GDOP  is  driven  by  the  error  in  altitude,  aircraft  velocity,  and  time.  Evaluating  the 
performance  of  E/O  systems  and  determining  a  cutline  for  the  E/O  system  mea¬ 
surement  uncertainty  needed  to  constructively  aid  a  navigation  grade  inertial  system. 
Testing  the  limits  of  the  E/O  system  was  not  in  the  scope  of  this  initial  contemporary 
driftmeter  replication. 

Increasing  the  fidelity  of  a  simulation  model  is  always  an  avenue  for  future  work. 
The  current  implementation  of  INS  error  sources  is  strictly  the  accelerometer  bias 
and  the  gyroscope  bias.  Including  the  drift  and  scale  factor  errors  would  increase  the 
fidelity  of  the  INS  error  model  but  are  not  a  necessity  for  an  understanding  of  the 
how  bearings-only  measurements  over  time  can  aid  the  INS. 
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List  of  Acronyms 


AFIT  Air  Force  Institute  of  Technology 

ANT  Autonomy  and  Navigation  Technology 

CCD  Charge  Coupled  Device 

CDF  Cumulative  Distribution  Function 

CENSURE  Center  Surround  Extremas 

DCM  Direction  Cosine  Matrix 

ECEF  Earth  Centered  Earth  Fixed 

EKF  Extended  Kalman  Filter 

ENU  East  -  North  -  Up 

E/O  Electro-Optical 

FOGM  First  Order  Gaussian  Markov 

GDOP  Geometric  Dilution  of  Precision 

GNSS  Global  Navigation  Satellite  System 

GPS  Global  Positioning  System 

IMU  Inertial  Measurement  Unit 

INS  Inertial  Navigation  System 

IP  Initial  Point 

KF  Kalman  Filter 
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LHS  Left  Hand  Side 


LOS  Line  of  Sight 

MEMS  Micro-Electrical  Mechanical  Sensors 
MMSE  Minimum  Mean  Squared  Estimator 
PDF  Probability  Distribution  Function 
PVA  Position  Velocity  Attitude 
RF  Radio  Frequency 
RHS  Right  Hand  Side 

SIFT  Scale  Invariant  Feature  Transformation 
SLAM  Simultaneous  Localization  and  Mapping 
SNR  Signal-to- Noise  Ratio 
SURF  Speeded  Lip  Robust  Transformation 
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